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I.  INTRODUCTION 

A.  MOTIVATION  AND  BACKGROUND 

Land  mines  are  an  inexpensive  and  effective  defensive  means  in  wars.  The 
problem  with  land  mines  is  that  they  remain  to  be  a  threat  when  wars  are  over. 
International  efforts  are  being  made  to  ensure  that  land  mines  deployed  in  the  future  are 
equipped  with  a  time-out  device,  and  mine  locations  are  properly  recorded.  While  such  a 
treaty  may  provide  relief  in  the  future,  millions  of  land  mines  were  planted  all  over  the 
world  as  a  result  of  wars  and  regional  conflicts  in  the  past. 

There  are  about  110  million  land  mines  scattered  around  the  world  in  more  than 
60  countries  —  most  of  them  in  the  Third  World  [Ref.  1,  2,  3,  4,  and  5].  These  land 
mines  kill  about  10,000  and  injure  another  20,000  people  (many  of  them  are  children) 
every  year.  Moreover,  there  are  millions  of  acres  of  the  US  formerly  used  defense  sites 
(FUDS)  that  are  contaminated  with  unexploded  ordnance  (UXO)  as  a  result  of  military 
testing  and  training  in  the  past  [Ref.  6].  The  contaminated  land  must  be  cleared  inch  by 
inch  before  transferring  to  civilian  use.  The  difficulties  of  these  clearing  missions  are  in 
the  variety  of  the  objects  to  be  identified  and  the  diversity  of  the  environments  that  are 
contaminated. 

B.  OBJECTIVES 

As  the  military  continues  to  downsize,  the  process  of  turning  the  land  over  to  the 
civilian  sector  is  sensitive,  intensive,  and  costly.  The  aforementioned  costs  are  both 
monetary  and  in  some  instance  the  loss  of  human  life.  One  of  the  most  complex 
problems  is  the  clearing  of  UXO's  from  the  FUDS. 

The  Department  of  Defense  (DOD)  has  recently  approved  two  organizational 
structures  to  confront  the  challenge  of  UXO  remediation  and  wide-area  de-mining.  The 
objective  of  the  first  committee  is  to  develop  fully  coordinated  requirements  driven 
research  and  development  program  for  countermine,  de-mining,  site  remediation,  range 
clearance,  and  explosive  ordnance  disposal.  Within  the  first  committee  there  is  a  specific 


group  focused  on  detection  technology.  The  second  committee  will  focus  on  current 
technologies  and  ways  to  improve  in  the  future.  One  of  the  phases  will  examine  current 
UXO  remediation,  active  range  UXO  clearance  and  explosive  ordnance  disposal  efforts. 
Hence,  the  UXO  problem  is  serious  and  a  highly  visible  issue  within  the  DOD.  The 
current  approach  to  mine  and  UXO  clearing  is  dangerous  and  labor  intensive  [Ref.  6].  In 
a  typical  UXO  clearing  scenario,  Explosive  Ordnance  Disposal  (EOD)  technicians  walk 
slowly  and  carefully  over  a  contaminated  field  in  an  attempt  to  identify  the  presence  of 
UXO's  that  may  be  fully  buried,  half  buried,  or  totally  on  the  surface.  UXO's  found  on 
the  surface  are  visually  examined  to  determine  their  types  and  fuse  mechanisms.  If  fuse 
mechanisms  are  recognized  and  the  condition  of  the  UXO  permits  (e.g.,  not  rusted, 
decayed,  or  encased  in  soil),  an  effort  is  made  to  defuse  UXO's.  UXO's  that  cannot  be 
defused  are  gathered  at  a  safe  location  and  are  destroyed  using  shaped  charges. 
Transporting  any  live  UXO  is  extremely  dangerous  and  the  motion  of  the  transport 
vehicle  must  be  gentle.  Moreover,  buried  UXO's  must  be  unearthed  first.  Therefore, 
UXO  clearing  consists  of  detection,  identification,  defusing,  excavating,  transporting,  and 
disposal.  Through  robotics  and  the  use  of  advanced  technology  it  should  be  possible  to 
make  UXO  clearing  tasks  safer,  cost-effective,  and  more  efficient. 

At  the  Naval  Postgraduate  School  a  team  has  been  put  together  to  develop  a 
semiautonomous  vehicle  or  robot,  which  will  survey  possible  contaminated  areas  for 
UXO's.  The  "rotary"  class  vehicle  by  its  very  design  is  capable  of  independent  driving 
and  steering  with  each  wheel.  Hence,  rotary  vehicles  are  capable  of  stronger  torque  and 
traction  than  most  other  vehicles  on  rugged  terrains.  Rotary  vehicles  with  two  wheels  (on 
a  very  smooth  surface)  have  been  shown  to  possess  the  aforementioned  enhanced  torque 
and  traction  -  with  an  increase  in  the  number  of  wheels  on  the  vehicle  the  capabilities  are 
significantly  improved.  The  vehicle  needs  to  be  highly  mobile  and  capable  of  producing 
very  fine  motion  to  negotiate  and  search  contaminated  sites.  To  fulfill  the  requirement  of 
the  special  wheel  architecture,  the  semiautonomous  vehicle,  called  Shepherd  (a  rotary 
vehicle),  is  presently  under  development.  Shepherd  possesses  stronger  torque  and 
traction   on   rugged   terrain,   because   of  its  special   architecture   (i.e.,   useful   for  the 


mine/UXO.  Mission).  The  name  "Shepherd"  was  given  to  the  vehicle  because  of  the 
protective  function  of  the  "shepherd"  in  many  ancient  cultures — hopefully  this  Shepherd 
will  also  save  lives.  Shepherd  (Figure  1.1)  is  a  four-wheeled  vehicle  with  independent 
steering  and  driving  capabilities.  The  four-wheel  independent  driving  and  steering 
capability  provides  Shepherd  a  high  level  of  mobility  and  preciseness  in  motion  control 
[Ref.  7]. 


Figure  1.1:  A  "rotary"  vehicle:  Shepherd. 


The  fundamental  objective  of  this  thesis  research  project  is  to  assist  in  the 
construction  of  a  user-friendly  and  high-precision  semiautonomous  robotics  tool  to  help 
the  unexploded  ordnance  (UXO)  and  mine  clearing  mission. 


This  thesis  will  examine  the  following  research  areas: 

•  What  kinematics  algorithms  must  be  developed  to  support  a  vehicle  with  three 
degrees  of  freedom  of  motion?  The  aforementioned  algorithms  must  support 
highly  flexible,  controlled,  and  precise  motion. 

•  What  types  of  controls  are  required  to  ensure  the  optimal  mix  of  driving  and 
steering  resources?  Moreover,  what  must  be  done  to  ensure  that  all  the 
resources  complement? 

•  How  can  the  knowledge  gained  in  the  aforementioned  research  areas  be  used 
to  develop  searching  motion? 

•  How  should  the  hardware  and  software  systems  be  implemented  to  support 
the  aforementioned  goals? 

•  How  can  human  operators  remotely  control  the  vehicle  through  an  appropriate 
interface? 


C.         ORGANIZATION 

This  chapter  provides  a  general  overview  of  traditional  and  current  techniques  for 
identifying  unexploded  ordnance.  Chapter  II  provides  the  System  Overview  and 
illustrates  the  concept  of  motion  control.  Chapter  HI  describes  the  Shepherd  Mobile 
Platform  and  the  On-Board  Computer  System.  Chapter  IV  presents  the  Shepherd 
Software  Description  and  places  great  emphasis  on  the  Shepherd  Real-time  Kernel 
(SRK).  In  Chapter  V  the  results  of  experiments  and  testing  motion  control  are  presented. 
Chapter  VI  explains  the  current  motion  modes  and  Chapter  VII  summarizes  the  thesis. 


II.  SHEPHERD  SYSTEM  DESIGN 

A.         SYSTEM  OVERVIEW 

In  consideration  of  aiding  the  UXO  task,  what  type  of  vehicle  should  be 
developed?  This  vehicle  will  face  difficulties  of  clearing  missions  in  the  variety  of  the 
objects  to  be  identified  and  the  diversity  of  the  environments  that  are  contaminated.  This 
vehicle  has  to  have  precise  and  smooth  motion,  display  motion  flexibility,  and  contain 
robust  motion  in  varied  environments  such  as  soft  soil  and  rough  terrain. 

The  vehicle  must  be  capable  of  precise  and  smooth  motion  while  searching  for 
UXO's.  The  very  nature  of  trying  to  locate  UXO's  should  be  meticulous  and  cautious. 
Haphazzard  and  jerky  motions  could  contribute  to  lost  of  the  vehicle  due  to  unwanted 
detonations.  Also,  motion  flexibility  is  absolutely  necessary.  This  will  enable  different 
approaches  or  techniques  for  locating  UXO's.  Finally,  the  motion  exhibited  must  be 
robust  and  stable  due  to  the  nature  of  UXO  environments.  While  traversing  these 
environments,  the  vehicle  should  not  lose  its  precise,  smooth,  and  flexible  motion 
characteristics.  For  these  reasons,  a  rigid  body  vehicle  with  at  least  2  steerable  wheels 
capable  of  semiautonomous  or  autonomous  motions  and  equipped  with  sensors  for 
detecting  UXO's  was  considered  at  the  abstract  level. 

So,  the  rotary  vehicle  platform  was  chosen,  with  the  addition  of  four  steerable  and 
drivable  wheels  and  a  powerful  computer  system  for  control.  The  four  wheels  have  thick 
tires  and  each  contains  two  motors,  one  for  driving  and  one  for  steering.  Because  of  this, 
three  degrees  of  freedom  motion  is  possible  which  allows  for  motion  flexibility.  The 
independently  driven  four  wheels  aids  in  providing  stronger  traction  than  any  other 
wheeled  vehicle  allowing  for  the  negotiating  of  uneven  slopes  ,  soft  soil,  or  rough  terrain. 
The  vehicle  itself  also  provides  the  capability  for  further  expansion  of  the  system  which 
will  give  it  the  full  capability  of  fulfilling  all  aspects  of  the  UXO  mission.  Chapters  III 
and  IV  expand  on  the  details  of  the  system  architecture  regarding  the  hardware  and 
software  of  this  rotary  vehicle. 


B.         MOTION  MODES 

Due  to  the  uniqueness  of  Shepherd's  independent  4-wheel  motion  of  360  degrees, 
several  modes  of  motion  are  possible.  The  possible  vehicle  motions  are: 

•  Tangential  —  the  vehicle's  change  in  direction  of  movement  is  equal  to  its 
change  in  heading. 

•  Constant  orientation  —  the  vehicle's  heading  is  constant  regardless  of  the 
change  in  the  direction  of  movement. 

•  Complex  which  falls  into  neither  of  the  above. 

•  Rotation 

At  this  stage  of  development  of  Shepherd  the  constant  orientation,  complex,  and  rotation 
motion  modes  have  been  implemented  with  work  proceeding  on  the  tangential  motion 
mode. 

The  user  interface  menu  has  a  list  of  specific  motions  designed  for  the  vehicle, 
which  encompasses  the  typical  vehicle  motions.  This  list  includes  1-Stop,  2-Straight 
motion  (autonomous),  3-Straight  motion  by  joystick,  4-XY-motion  by  joystick,  5-Rotate, 
6-Sinusoidal,  7-Tornado  (external),  8-Tornado  (internal),  9-Tangential,  0-Exit,  a- 
Tangential  motion  II,  and  t-Test  motion.  The  sinusoidal  motion  is  an  implementation  of 
the  constant  orientation  while  the  tornado  motions  are  an  implementation  of  the  complex 
vehicle  motion.  Both  tangential  motions  are  an  attempt  to  implement  the  tangential 
vehicle  motion.  Further  coverage  will  be  given  to  several  of  these  motions  in  more 
detail. 


III.  SHEPHERD  SYSTEM  HARDWARE 

A.  OVERVIEW 

The  Shepherd  system  hardware  consists  of  the  mobile  platform  and  the  shepherd 
on-board  computer  system.  The  mobile  platform  is  the  "mechanical"  part  of  Shepherd, 
which  provides  motion  and  is  directed  by  the  on-board  computer  system.  The  Shepherd 
on-board  computer  system  provides  the  computing  power  required  controlling  and 
directing  Shepherd.  Figure  3.1  provides  a  global  perspective  of  the  Shepherd  System 
Hardware.  Shepherd  has  four  wheels,  which  are  controlled  independently.  Each  wheel 
has  two  motors;  one  for  driving  the  wheel  and  the  other  for  steering.  Moreover,  the 
steering  capability  for  each  wheel  exceeds  360  degrees.  The  maximum  driving  speed 
(determined  empirically)  is  approximately  87  centimeters/second.  The  unique  mix  of 
driving  and  steering  capability  is  what  provides  the  challenge  and  motion  flexibility  of 
this  vehicle. 

Shepherd  has  a  mass  of  150  kilograms  and  is  built  to  form  a  square  chassis 
(frame).  Shepherd's  wheels  are  centered  on  the  corners  of  the  square  leading  to  very 
elegant  calculations,  as  you  will  see  later.  Shepherd's  Alternating  Current  (AC)  electric 
motors  are  powered  by  12  batteries,  which  are  charged  by  an  external  AC  source  through 
converter.  Figure  3.1  is  also  a  transparent  view  of  Shepherd  from  above  which  shows  the 
Central  Processor  Unit  (CPU),  input/output  (I/O)  boards,  servoamplifiers,  batteries,  and 
wheel  unit  assembly. 

B.  THE  MOBILE  PLATFORM 

The  mobile  platform  consists  of  the  vehicle  body.  The  vehicle  body  includes  the 
vehicle's  frame,  motors,  encoders,  servocontrollers,  gears,  wheels,  tires,  and  power 
supply.  The  on-board  computer  system  is  not  considered  part  of  the  mobile  platform. 


CPU  and  I/O       Servoamplifiers   Batteries 


Wheel  assembly 


Figure  3.1:  A  transparent  view  of  Shepherd  from  above.  Showing  the  "relative"  location  of  the 
CPU,  and  I/O  boards,  servoamplifiers,  batteries,  and  wheel  assembly.  The  relative  position  of  the 
vehicle  components  is  subject  to  change  as  the  vehicle  is  modified. 


1. 


Motors  and  Encoders 


The  eight  motors  and  their  corresponding  shaft  encoders  used  in  Shepherd  are 
from  Yamayo  Electric,  Inc.  These  motors  allow  Shepherd  to  reach  a  "theorectical" 
maximum  driving  speed  of  4  kilometers  per  hour  (km/h),  and  a  rate  of  1  revolution  per 
second  about  the  steering  axis  (Ref.  7).  Figure  3.2  provides  the  characteristics  for  the 
driving  and  steering  motors. 


Servomotor  Characteristics 

Driving  Motor 

Steering  Motor 

Nominal  Torque 

1.274  N-m 

0.32  N-m 

Maximum 
Torque 

3.84  N-m 

0.98  N-m 

Nominal 
Rotation  Rate 

3000  rpm 

3000  rpm 

Maximum 
Rotation  Rate 

4500  rpm 

4500  rpm 

Size 

60  X  123.5  mm 

54  X  86  mm 

Weight 

1.7  Kg 

0.74  Kg 

Power  (AC) 

400  W 

100  W 

Figure  3.2:  Servomotor  characteristics  for  Shepherd. 

Figure  3.3  illustrates  the  relative  motor  (M1-M7)  position  on  the  vehicle  and  the 
general  orientation  of  the  vehicle  (e.g.,  front). 


Shepherd  Rotary  Vehicle 
Motor  diagram 


□   *-    □ 

M2-Dnve         ^ 

*          Ml -Drive 

M6- Steer 

M5-Steer 

M4-Dnve 

M3-Drive 

M8- Steer 

M7- Steer 

□ 

□ 

Figure  3.3:  Shepherd  motor  diagram 


2.  Servomotor  Controllers 

The  servomotor  controllers  actually  provide  the  commanded  voltage  and  current 
to  the  driving  and  steering  motors  to  effect  motion.  The  importance  of  the  servomotor 
controllers  can  not  be  understated;  the  values  written  to  the  digital  output  board  (and  read 
from  the  digital  input  board)  are  within  the  acceptable  range  of  the  controllers.  Also,  the 
interface  specification  matches  the  range  for  the  motors  used  on  Shepherd  and  input 
signal  voltage  corresponds  to  the  range  allowed  for  the  driving  and  steering  motors.  Later 
sections  of  this  document  will  show  that  the  voltage  produce  by  the  batteries  is 
approximately  144  volts,  which  is  within  the  acceptable  range  of  the  controllers.  Figure 
3.4  contains  the  characteristic  and  interface  data  for  the  servomotor  controllers. 


Servo  Motor  Controller  Specifications 

Motor  Capacity 

400  W 

Interface  Specification 

3000  rpm/5000  rpm 

Output  Current 

8A 

Control  Method 

PWM 

Input  Control  Voltage 

DC+  120-150  V 

Input  Signal  Voltage 

DC+/-  10V 

Input/Output  Signal 

8  bit 

Figure  3.4:  Servomotor  controller's  characteristic  and  interface  data. 

3.  Gears 

Shepherd's  reduction  gear  system  contains  flat  gears,  planetary  gears,  and  bevel 
gears.  This  gear  configuration  has  a  1 :50  gear  ratio  for  both  driving  and  steering  [Ref.  8]. 
Figure  3.5  provides  a  transparent  view  of  the  flat  gears  in  the  wheel  assembly.  Due  to  the 
gear  configuration,  when  the  wheels  are  used  for  steered  then  some  driving  is  also 
initiated.    The  aforementioned  driving  is  cancelled  by  applying  the  required  amount  of 
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"opposite"  driving.    And  this  "opposite"  driving  is  based  on  the  1:50  gear  ratio  and  is 
handled  in  the  Shepherd  code  (Appendix  J  Consolidated  header  files,  line  360). 

Figure  3.6  is  a  side  cut  away  of  the  wheel  assembly.  This  cut  away  shows  the 
gears  involved  in  transferring  force  from  the  motors  to  the  wheels.  Gear  ratio  and  force 
calculations  can  be  obtained  from  [Ref.  8].  Also,  mounted  one  of  the  flat  gears  is  a  "hall" 
sensor,  which  Shepherd  uses  to  determine  if  wheel  is  aligned. 


Planetary  gears  and  flat  gears 


Figure  3.5:  Flat  gears  in  the  wheel  assembly  (transparent  view  from  the  top) 
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Driving  Motor 


Figure  3.6:  Side  cut  away  of  the  motors  and  gears  used  in  the  Shepherd  driving  and 
steering  mechanism  (diagram  not  to  scale). 


4. 


Wheels  and  Tires 


Shepherd's  wheel  diameter  is  400  millimeters  (mm),  and  the  suspension  travel 
range  is  100  mm.  Shepherd  has  somewhat  ruggedized  tires,  with  a  tire  friction  factor  of 
.5.  The  maximim  tire  pressure  has  been  calculated  as  49.8  pounds  per  square  inch  (psi); 
where  36  psi  is  currently  used. 
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5.  Power  Supply  System 

Shepherd's  power  supply  consists  of  twelve  (12-volt)  batteries  connected  serially. 
The  voltage  generated  from  the  batteries  is  between  144-150  volts  (within  the  servomotor 
specifications).  The  batteries  have  been  used  for  periods  up  to  two  (2)  hours  without  any 
noticeable  degradation  of  performance.  Figure  3.7  shows  the  switches  required  for  the 
operational  settings  of  charge,  run  (battery),  run  (external)  and  run  (external/charge).  The 
following  are  valid  switch  settings  for  Shepherd. 

•  Charge:   1  -SW-C  OFF 

2  -SW-B  ON 

3  -SW-A  ON 

•  Run  (battery):   1  -SW-A  OFF 

2  -SW-C  ON 

3  -SW-B  ON 

•  Run  (external):   1  -SW-B  OFF 

2  -SW-C  ON 

3  -SW-A  ON 


Run  (external/charge):   1  -SW-A  ON 

2  -SW-B  ON 

3  -SW-C  ON 


Shepherd  Rotary  Vehicle 
Power  Supply 

0 

AC/DC  Converter 

B  Y 

Shepherd 
Rotary  Robot 

V 

_L 
.         1 

1 

Figure  3.7:  Shepherd  power  supply  switch  diagram 
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Figure  3.8:  Simplified  schematic  diagram  of  Shepherd  power  supply.  Showing  both  an  external 
AC  source  and  the  12-volt  batteries  serially  connected. 

An  AC  source  (1 15-Volts)  can  be  used  to  run  Shepherd  or  to  charge  the  batteries 
(accomplished  by  the  AC/DC  converter).  Figure  3.8  is  a  global  schematic  of  the 
Shepherd  power  supply  [Ref.  9]. 


C.         SHEPHERD  ONBOARD  COMPUTER  SYSTEM 

The  Shepherd  vehicle's  system  design  is  illustrated  by  Figures  3.9,  and  is  broken 
down  into  the  hardware  and  software  components  both  of  which  will  be  explained  in 
greater  detail  in  later.  The  hardware  system  is  a  combination  of  the  mobile  platform,  an 
on-board  computer  system,  servo  drivers,  batteries,  and  a  laptop  computer  for  a  real-time 
I/O  device.  The  computer  system  consists  of  a  Taurus  board  housing  two  Motorola 
CPUs  which  are  68040  and  68030,  a  digital  to  analog  board,  a  digital  input  board,  a 
digital  output  board,  a  digital  counter  board;  and  a  Versa  Module  European  bus 
(VMEbus)  based  on  Motorola  architecture.  Servo-controllers  are  connected  to  these  I/O 
boards.  Motor  encoders  are  connedcted  to  the  counter  board. 
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UNIX  System 


Ethernet 


Laptop 
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VMEBUS 


Digital 
Output 
Board 
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Encoders 
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Z555 


Digital  to 
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Counter 
Board 


Taurus 
Board/ 

M68040 


Figure  3.9:  Diagram  of  the  Shepherd  on-board  computer  system. 
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1. 


Taurus  Board 


The  Taurus  is  a  dual-processor,  and  dual  bus  architecture,  VME  single  slot,  single 
board  computer  [Ref.  11].  The  primary  computing  engine  is  a  Motorola  68040  processor 
running  at  25  MHz.  The  second  processor  on  Taurus  is  the  Motorola  68030.  Although, 
taurus  supports  several  real-time  operating  systems,  an  in-house  operating  kernel,  SRK, 
was  developed  (chapter  IV).  Taurus  also  takes  advantage  of  the  direct  memory  access 
(DMA)  functions  provided  by  Ethernet,  SCSI,  and  Intelligent  Serial  Controllers  to  DMA 
into  main  direct  access  memory  (DRAM)  through  an  isolation  gateway  between  the 
M68040  bus  and  the  M68030  bus.  Moreover,  Taurus  acts  as  a  fully  functional  VMEbus 
controller  and  may  operate  in  Slot  1  of  the  VMEbus  back  plane  (this  is  the  case  for 
Shepherd).  Hence,  the  Taurus  board  is  a  powerful  VMEbus  engine  and  supports  the 
requirements  for  a  real-time  operating  system  and  a  completely  self-contained  computing 
environment.    Taurus  features: 


25  MHz  M68040  Processor 

Burst  Transfers 

Ethernet  and  SCSI  with  on  chip  DMA 

1 6  Megabytes  of  DRAM  main  Memory 

4  Megabytes  of  EPROM 

1  Megabyte  of  Flash  EPROM 

25  MHz  M68030  Processor 

6  Serial  Ports:  4  (RS-232-D  Intelligent  Ports  with  DMA),  and  2  using  a 

68c681  device 

32  Parallel  I/O 

11,  16  bit  Timers  (cascadeable  into  combinations  of  up  to  80  bits) 

Interprocessor  Mailbox 

Dynamic  Bus  Sizing 

Real-time  clock  with  battery  back  up 

Watchdog  Timer  and  8  KB  of  battery  back-up  Static  RAM 
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Why  was  the  Taurus  board  chosen  for  this  project?  In  addition  to  the 
aforementioned  characteristics,  the  board  uses  the  M68040  processor.  In  previous 
development  of  the  Yamabico-11  [Ref.  12]  robot,  the  M68020  was  used.  Motorola 
claims  that  its  M68000  series  chips  are  backward  compatible.  This  research  has 
concluded  that  this  is  mostly  true,  however  on  some  key  issues  (math  functions  and 
assembly  code)  this  has  not  proven  to  be  the  case.  These  issues  will  be  revisited  in 
chapter  IV.  Again  a  key  asset  of  the  Taurus  board  is  how  elegantly  and  logically  internal 
and  external  interrupts  are  handled  (Figure  3.10). 


VMEbus  Interrupts 

o 


Interrupts  from 
On-Board  Devices 

-SCSI 

-Ethernet 


VIC068/VIC64 


[RQ-6 


M68030 


Figure  3.10:  Interrupt  handling  diagram  for  the  Taurus  board  [Ref.  11] 

The  Taurus  board's  communications  facility  adds  flexibility  to  the  implementation 
of  RS-232  or  Ethernet.  The  M68040  chip  is  a  very  versatile  processor  and  powerful 
processor,  and  can  perform  14  different  operations  at  a  given  time.  That  is  6  operations 
by  the  Integer  Unit  (IU),  3  operations  by  the  Floating-Point  Unit  (FPU),  4  by  the  Memory 
Management  Unit  (MMU),  and  a  Bus  interface  operation.  The  number  of  timers  on  the 
Taurus  board  is  also  of  a  great  benefit,  moreover  Timer  5  will  be  discussed  in  chapter  IV. 
The  M68040  is  highly  parallel,  with  a  6-stage  integer  pipeline,  that  when  filled,  will 
execute  an  instruction  for  every  clock  cycle.  Moreover,  each  of  the  MMUs  can 
accomplish  a  cache  access  and  address  translation  concurrently. 
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2.         Digital  To  Analog  Board 

The  Acromag  Series  9210  Analog  Output  Board  (AVME9210)  provides  the 
means  for  connecting  and  driving  analog  circuits  with  outputs  from  the  VMEbus  for  the 
Shepherd  system  [Ref.  13].  The  board  has  8  channels;  each  channel  has  a  12-bit 
resolution.  A  DAC  per  channel  is  used  for  signal  accuracy.  The  DACs  are  set  up  to 
accept  either  straight  binary  or  two's  compliment  data.  The  board  has  five  programmable 
ranges  for  output  voltages;  however,  +/-  10- Volts  will  be  used  with  shepherd  because  it  is 
a  direct  mapping  to  the  maximums  for  the  servomotors  inputs.  Characteristics  of  the 
AVME9210  are  as  follows: 


12  bit  output  resolution 

individual  DAC  per  channel 

8  channels  per  board 

Byte  or  Word  data  transfers 

Power  up  reset 

Pass/Fail  status  indicators  on  the  front  panel 


DAC  Data  Register 

Data  Register 

Base  Address  Offset 

Channel  0 

+82H 

Channel  1 

+84H 

Channel  2 

+86H 

Channel  3 

+88H 

Channel  4 

+8AH 

Channel  5 

+8CH 

Channel  6 

+8EH 

Channel  7 

+90H 

Figure  3.11:    DAC  data  register,  8  channels  of  output.   Each  channel  is  a  digital  to  analog  converter. 
A  two  byte  address  is  reserved  for  each  data  register. 

A  single  channel  represents  a  driving  or  steering  motor  in  Shepherd  [Appendix  E, 
Motor.c,  line  228].  The  status  control  register  controls  the  pass/fail  light.   The  Shepherd 
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code  toggles  the  pass  fail  light  in  some  instances  to  ensure  the  system  and  code  are 
functioning  properly.  There  is  a  memory  location  for  the  board  status  indicator  flags  and 
reset.  This  memory  location  is  one  byte  in  length  and  is  located  at  base  address  +81H. 
During  the  early  manual  testing  this  status  control  register  was  used  exclusively  to 
accomplish  resetting  the  AVME9210.  Also,  each  of  the  aforementioned  two  byte  DAC 
data  registers  (Figure  3.12)  are  set  up  as  follows: 


MSB 

LSB 

Dll 

D10 

D9 

D8 

D7 

D6 

D5 

D4 

D3 

D2 

Dl 

DO 

X 

X 

X 

X 

12  bits 

4  bits 

Bit  DAC  Data 

Undefined 

Figure  3.12:    DAC  data  register  (16  bit).   Most  significant  bit  (MSB)  and  least  significant  bit  (LSB). 
The  12  bits  are  a  direct  mapping  of  input  values  to  the  board,  integer  range  [1023,  -1024]. 


The  AVME9210  is  located  at  base  address  0xffff0400  and  is  represented  in  the 
Shepherd  code  by  the  "label"  VME9210. 


Motor  and  DA  Board  Address  Ma 

pping 

Driving  Motor  and 

Steering  Motor  and 

Address 

Address 

Wheel  1 

Ml 

M5 

VME9210  +  0x0082 

VME9210  +  0x008A 

Wheel  2 

M2 

M6 

VME9210  +  0x0084 

VME9210  +  0x008C 

Wheel  3 

M3 

M7 

VME9210  +  0x0086 

VME9210  +  0x008E 

Wheel  4 

M4 

M8 

VME9210  +  0x0088 

VME9210  +  0x0090 

Figure  3.13:   AVME9210  address  to  "physical"  motor  mapping. 
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3.  Digital  Input  Board 

The  Acromag  Series  9421  Isolated  Digital  Input  Board  (DIB)  provides  the  means 
for  connecting  the  Digital  DC  inputs  the  VMEbus  for  the  Shepherd  system  [Ref.  14]. 
The  DIB  board  isolates  all  digital  inputs  from  the  VMEbus  for  up  to  250V  AC,  or  350V 
DC  on  a  continuous  basis  (falls  within  the  constraints  of  the  Shepherd  servomotors).  The 
pass/fail  light  on  this  board  is  similar  to  the  one  used  in  the  digital  to  analog  board 
previously  mentioned.  And  the  DIB  also  has  the  input  channel  on  light  as  well.  The 
board  has  64,  1  bit  channels  configure  as  four,  16  bit  words.  The  inputs  can  be  bi-polar 
(with  polarity  being  +/-  or  -/+  at  either  end  of  the  channel).  The  bi-directional  polarity 
allows  Shepherd  to  use  this  for  changing  the  direction  of  wheel  driving  or  the  direction  of 
steering  with  a  change  of  input  polarity.  The  DIB  has  the  base  address  of  OxffffOOOO  and 
the  "label"  VME9421  is  used  in  the  code  [Appendix  J,  Consolidated  header  files,  line 
386]. 


4.  Digital  Output  Board 

The  Microsystems  International  Corporation  32-bit  Optically  Coupled  Digital 
Output  Board  (VMIVME-2170A)  consists  of  VMEbus  compatibility  logic,  data  output 
control  logic,  four  8-bit  output  registers,  and  32  bits  of  isolated  outputs.  The  VMEbus 
logic  contains  address  decoding  logic  and  data  transfer  control  logic,  which  provides  for 
8-  or  16-  bit  data  transfers  in  the  "short"  I/O  address  space.  The  data  output  control  logic 
selects  byte  or  word  transfers  to  the  32  optically  isolated  channels.  The  Shepherd 
research  group  spent  many  hours  attempting  to  master  this  logic — a  key  problem  was 
how  to  determine  where  the  least  significant  value  was  for  each  data  register.  However, 
Thorsten  Leonardy's  efforts  paved  the  way  for  a  consistent  and  logical  method  of  writing 
to  the  aforementioned  registers.  From  this  the  Shepherd  group  was  able  to  selectively 
choose  combinations  of  motors  for  steering  and  driving  [Appendix  J,  Consolidated 
header  files,  lines  181-192].  Figure  3.14  shows  the  register  bit  definitions  [  Ref.  15]. 
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$XXXO  Data  Register 

0 

Bit  15 

Bit  14 

Bit  13 

Bit  12 

Bit  11 

Bit  10 

Bit  9 

Bit  8 

Output  Data 

OD31 

OD30 

OD29 

OD28 

OD27 

OD26 

OD25 

OD24 

$XXX1  Data  Register 

1 

Bit  7 

Bit  6 

Bit  5 

Bit  4 

Bit  3 

Bit  2 

Bit  1 

BitO 

Output  Data 

OD23 

OD22 

OD21 

OD20 

OD19 

OD18 

0D17 

OD16 

$XXX2  Data  Register  2 

Bit  15 

Bit  14 

Bit  13 

Bit  12 

Bit  11 

Bit  10 

Bit  9 

Bit  8 

Output  Data 

OD15 

OD14 

OD13 

OD  12 

OD11 

OD10 

0D9 

OD8 

$XXX3  Data  Register 

3 

Bit  7 

Bit  6 

Bit  5 

Bit  4 

Bit  3 

Bit  2 

Bit  1 

BitO 

Output  Data 

OD7 

OD6 

OD5 

OD4 

OD3 

OD2 

OD1 

ODO 

Figure  3.14:  Register  Bit  Definitions 

The  output  board  has  the  base  address  of  OxffffffOO,  which  is  represented  in  the 
code  by  the  "label"  VME2170. 


VME2170  Bit  Assignment 

Register  3 

Register  2 

Register  1 

Register  0 

OOOO 

0000 

1001           0C10 

0100           1001 

0010 

0100 

J 


J  L 


JU 


Umlsed         Wheel  4  Wheel  3  Wheel  2  Wheel  1     Wheel  4  Wheel  3  Wheel  2  Wheel  1 
Steering  Steering  Stppring  Steering    Driving    Driving  Driving    Driving 


Figure  3.15:  VME2170  bit  assignment.  Note  3  bit  assignment  for  driving  or  steering  motor  selection. 
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Figure  3.15  indicates  that  "masks"  could  be  written  to  the  VME2170  base  address 
and  used  for  motor  selection.  Hence,  writing  a  mask  for  0x00000004  would  select,  motor 
1  (M 1 )  in  wheel  1  for  driving.  And  writing  a  mask  for  0x00004000  would  select,  motor 
5  (M5)  in  wheel  1  for  steering.  Moreover,  using  a  mask  for  0x00924924  all  motors  can 
be  selected  (using  a  "logical  And"  on  the  true  values  for  each  motor  selected). 


5.  Counter  Board 

Shepherd  uses  the  Green  Spring  IP-Quadrature  Four  Channel  Quadrature  Decoder 
or  counter  board  [Ref.  16].  The  counter  board  reads  the  signal  produced  by  the  encoders 
(see  section  EQ.B.l  of  this  document);  this  signal  is  "index"  pulse  once  per  revolution  to 
provide  absolute  position  information.  There  are  four  channels  on  the  counter  board; 
each  channel  has  three  inputs.  The  inputs  are  normally  called  X,  Y,  Z,  and  the  board 
inputs  are  X  and  Y.  Z  is  the  control  or  index  input.  Each  channel  has  a  24-bit  up/down 
counter  block,  24-bit  capture/match  register  and  a  24-bit  output  latch  allow  for  an 
accurate  "on  the  fly"  reading  of  Quadrature  position  values.  The  up/down  count  direction 
in  the  counter  board  is  controlled  by  the  relative  phase  of  the  X  and  Y  inputs.  Count 
direction  can  be  reversed  by:  reversing  the  mechanical  motion  direction,  reverse  the 
connections  for  X  and  Y,  reverse  the  X  polarity  bit,  or  reverse  the  Y  polarity  bit. 
Shepherd  uses  a  function  called  "readEncoders"  which  is  an  excellent  example  of 
completing  three  consecutive  8-bit  reads  from  the  counter  board  and  catenation  the  8-bit 
segments  into  a  single  24-bit  position  reading  [Appendix  E:,  Motor.c,  lines  300-356  and 
606-620].  The  counter  board's  base  address  address  is  0xffff6000,  and  is  represented  by 
the  label  VMECTR1  in  the  Shepherd  code. 
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IV.  SHEPHERD  SOFTWARE  SYSTEM 

A.    OVERVIEW 

The  Shepherd  software  system  consists  of  the  software  development  environment 
(including  GCC  version  2.7.2.1  compiler),  Shepherd  Real-time  Kernel  (SRK),  and  the 
firmware  on  the  Taurus  board.  Shepherd  software  is  developed  in  the  "C"  language  on  a 
Unix  workstation,  and  the  code  is  cross-compiled  using  switches  with  the  GCC  compiler 
to  ensure  viability  on  the  Taurus  board  (step  by  step  instructions  are  in  the  Shepherd 
Operating  Manual,  Appendix  K).  The  code  is  in  "S"  record  format  and  is  transferred  (via 
Ethernet)  to  a  laptop  computer,  which  is  used  as  the  user  interface.  When  the  user  is 
ready  to  test  or  run  a  program,  it  is  then  download  via  RS232  to  the  RAM  on  the  Taurus 
board  using  the  Taurus  bug  or  firm-ware  on  the  Taurus  board.  The  SRK  has  real-time 
timer  control.  The  timer  interrupts  are  set  for  10  milliseconds,  but  can  be  modified  to 
suite  user  needs  and  requirements.  SRK's  central  motion  control  sections  are  shepherd. c, 
user.c,  and  the  driver  routine  (all  of  which  will  be  discussed  in  great  detail  later).  The 
user  interface  viewed  from  the  laptop  computer  is  also  generated  from  the  code  in  user.c 
and  other  I/O  code  segments.  The  overall  software  environment  and  firmware  works 
together  to  form  a  tightly  coupled  and  low  overhead  "operating  kernel"  that  is  the  SRK. 
The  use  of  the  SRK  allows  the  user  to  control  (i.e.,  timer  interrupt,  motion  control,  and 
user  interface)  the  Shepherd  rotary  vehicle. 


B.         SOFTWARE  ENVIRONMENT 

The  software  environment  (minus  the  SRK)  includes  the  following:  Bug  monitior, 
S-records,  and  Software  development  environment. 

1.  Bug  Monitor 

The  "taurus  bug"  is  a  powerful  debugging  and  evaluation  tool,  and  is  firmware  on 
the  Taurus  board  [Ref.  20].  It  has  facilities  for  loading  and  executing  user  programs 
under  complete  operator  control  and  evaluation  (and  is  used  extensively  in  Shepherd 
development).  The  "taurus  bug"  includes  commands  that  allow  the  user  to  display 
memory,  modify  memory,  set  and  remove  break  points,  an  assembler/disassembler,  and  a 
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system  self  test  capability  that  verifies  system  integrity  upon  power  up.  The  "taurus  bug" 
also  has  various  routines  to  handle  some  I/O,  string  functions,  and  data  conversion  via  the 
TRAP  #15  handler  on  the  Taurus  board  (used  in  Appendix  C,  lines  535-548).  Moreover, 
on  power  up,  all  static  variables  are  set  to  default  states,  the  break  point  table  is  cleared, 
all  target  registers  invalidated,  I/O  character  queues  cleaned,  the  vector  interrupt  table  is 
written  to  RAM,  and  all  on-board  devices  (serial  ports,  timers,  etc.)  are  cleared  or  reset. 
Taurus  bug  also  has  a  system  reset  and  abort  feature.  The  system  reset  completely  re- 
initializes the  board  and  the  abort  feature  captures  a  snapshot  of  the  processors  present 
state — allowing  the  use  of  stack  pointers,  and  the  program  counter  to  help  determine 
errors  (the  hardest  way  to  debug).  The  "taurus  bug"  was  a  very  valuable  tool;  however, 
at  times  it  was  difficult  to  use.  And  tracing  through  assembly  code  to  resolve  a  problem 
using  the  "Trace"  function  and  the  symbol  table  can  take  numerous  man-hours  -without 
immediately  yielding  a  positive  result.  A  very  important  function  of  "taurus  bug"  is  the 
loading  capability.  The  use  of  the  "Lo"  command  to  place  executable  code  in  memory  is 
key  in  the  development  process.  The  "S"  records  are  downloaded  to  the  Taurus  board  in 
this  manner. 


2. 


"S"  Records 


The  S-record  format  was  devised  by  Motorola  for  output  modules.  Its  key 
purpose  was  encoding  programs  or  data  files  in  a  printable  format  for  the  transportation 
between  computer  systems.  Hence,  providing  a  way  of  visiually  monitoring  the 
transportation  processs  and  a  method  of  quicky  editing  the  code  if  required.  S-records 
are  character  strings  made  of  several  fields  which  identify  the  record  type,  record  length, 
memoery  address,  code  data  and  checksum  (see  Figure  4. 1 ). 


Field 

Printable  Characters 

Contents 

Type 

2 

S0-S9 

Record  Length 

2 

Character  pairs  in  record 
minus  type 

Address 

4,  6,  or  8 

The  2,  3,  or  4  byte  address 
at  which  the  data  is  loaded 
into  memory 
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Field 

Printable  Characters 

Contents 

Code/Data 

0-2n 

From  zero  to  2n  bytes  of 
executable  code, 
descriptive  information, 
or  loadable  data. 

Checksum 

2 

Least  significant  byte  of 
l's  complement  of  the  sum 
of  the  values  represented 
by  the  pairs  of  characters 
making  up  records  length, 
address  and  the  code/data 
fields.  Used  for  error 
checking 

Figure  4.1:  S-record  content  chart  [Ref.  20]. 

S-records  module  may  contain  the  following  types  (and  many  more):  SO  (header 
data),  record  containing  address  where  code  is  to  reside  in  memory  (SI,  SI,  or  S3),  S5 
(the  number  of  records  transmitted  per  block),  and  the  termination  record  (S7,  S8,  or  S9). 
A  typical  S-record  might  look  like  this: 

S00600004844521B 

S113000284F245F2212226A000424290008237C21 
SI  1300100002000800082629001853812341001813 
S9030000FC 

A  detailed  byte  wise  explanation  of  S-records  is  contained  in  Ref.  20.  The  S-records  for 
shepherd's  development  are  generated  during  the  linking  and  loading  process.  A  special 
switch  is  used  that  allows  the  creation  of  a  file  named  "shepherd.TXT".  The 
"shepherd.TXT"  file  contains  the  S-records  to  be  downloaded. 
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3.  Software  Development  System 

As  mentioned  in  this  section's  overview  the  compiler  used  is  the  GCC  2.7.2.1. 
This  compiler  posed  many  problems  for  SRK  development.  One  of  the  biggest  obstacles 
was  the  passing  of  "composite"  structures.  Structure  values  had  to  be  placed  into  dummy 
variables  (Appendix  D,  lines  138-140)  in  order  to  get  the  code  to  execute.  This  was 
especially  odd  because  the  code  compiles,  but  will  not  execute  (if  the  dummy  variables 
are  not  used).  Also,  several  of  the  compiler  switches  that  supposedly  allow  mathematical 
code  (that  was  previously  valid  for  the  M68020  with  math  co-processor)  would  simply 
not  compile.  An  inordinate  amount  of  time  was  spent  trying  to  resolve  this  issue  because 
one  of  the  initial  precepts  of  the  project  was  that  old  code  from  the  Yamabico-1 1  robot 
system  would  be  portable.  There  are  still  anomalies  that  work  arounds  were  developed 
for.  For  instance,  there  is  a  "square  root"  function  that  compiles  and  works  in  every 
environment  (other  compilers)  yet  would  never  function  when  compiled  using  GCC 
2.7.2.1.  Moreover,  because  the  compiler  does  not  have  libraries  for  I/O  or  standard  math 
functions,  these  had  to  be  derived  to  support  the  M68040  and  Taurus  board  (again  taking 
an  inordinate  amount  time).  The  compiler  did  serve  its  purpose — because  its  freeware 
(the  budget  did  not  allow  for  commercial  compilers). 

One  switch  that  did  work  was  -m68040,  which  allowed  the  generation  of  M68040 
specific  code.  The  "makefile"  makes  use  of  the  -m68040  (segment  below): 

shepherd. o  :  shepherd. c 

gcc  -c  -m68040  -o  shepherd. o  shepherd. c 

timer.o  :  timer.c 

gcc  -c  -m68040  -o  timer.o  timer.c 

user.o  :  user.c 

gcc  -c  -m68040  -o  user.o  user.c 

motor.o  :   motor.c 

gcc  -c  -m68040  -o  motor.o  motor.c 
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The  early  testing  also  required  a  complete  understanding  of  the  "a.out"  generated 
from  the  assembler  and  the  link  editor  (the  link  editor  makes  "a.out"  executable  files). 
The  "a.out"  (Figure  4.2)  consists  of:  a  header,  program  text,  program  data,  text  and  data 
relocation  information,  a  symbol  table,  and  a  string  table  (the  last  three  sections  may  be 
omitted  if  the  program  is  loaded  with  the  -s  option.  All  of  the  aforementioned 
information  was  useful  because  our  earliest  a.outs  were  in  the  wrong  format,  hence  not 
useable  by  the  M68040  due  to  using  non-functioning  compiler  switches. 


A.OUT 

Ma^c  Number  |      Size  text  !  Size  data  j  Other 

M               0  Header  data 

TEXT 

DATA 

BSS  STACK 
SEGMENT 

Figure  4.2:  A  generic  A.OUT  format.  Note  many  features  of  the  A.OUT 
are  left  off,  such  as  symbol  table,  entry  point,  dynamic,  and  machine  type. 

The  final  element  to  be  discussed  is  how  the  link  editor  is  used.  First,  we  must 
discuss  the  DRAM  Memory  map  (Figure  4.3).  The  Taurus  board  documentation  [Ref. 
21]  warns  that  accessing  the  memory  below  $10000  (Hex),  hence  the  memory  the  format 
used  in  Figure  4.3  with  16  Megabytes  as  the  upper  bound. 
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DRAM  Memory  Map 

$100000 

or 
16MB 

END  DRAM 

a 

Region  Available  for 
User  program 

$10000     - 

$0    - 

" 

Start  user  DRAM 
Start  DRAM 

Region  used  by 
"taurus  bug" 

Figure  4.3:  DRAM  memory  map.  It  should  be  noted  that  to  be  able  to  write  to 
the  DRAM  the  Parity  ISM  register  must  be  disabled  or  the  block-  fill  command 
(taurus  bug)  used  to  clear  the  region  required  [Ref .  12,  and  22].  A  detailed 
example  is  in  the  Shepherd  Operating  Manual  (Appendix  K). 

Again  the  Shepherd  makefile  is  illustrative  of  the  linking  process: 


comp:  startup. o  shepherd. o  timer.o  serial. o  math.o  utils.o  utils030.o  user.o\ 
motor,  o  movement,  o 

Id  -Ttext  0x10000  -Tdata  0x20000  -Tbss  0x30000  -Map  shepherd.map 
oformat  srec\ 

-o  shepherd.TXT  startup. o  shepherd. o  timer.o  serial. o  math.o  utils.o\ 
utils030.o  user.o  motor.o  movement.o 

The  make  file  shows  the  text  segment  of  the  code  being  loaded  at  0x10000,  the  data 
segment  of  the  code  loaded  at  0x20000,  and  the  upper  bound  for  the  data  at  0x30000. 
Hence,  the  code  is  loaded  within  the  parameters  required  by  the  memory  map.  Earlier  it 
was  mentioned  that  the  S-records  were  generate  by  the  linker — the  "oformat  srec  -o 
shepherd.TXT"  generates  the  required  S-records  for  download  to  the  laptop  PC.  If  the 
"oformat  srec"  switch  is  not  used  then  the  standard  a.out  will  be  generated.    The  -Map 
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switch  also  allows  the  user  to  generate  a  symbol  table  (called  shepherd.map  here)  for  use 
in  debugging.  At  this  point  all  the  underlying  structure  for  SRK  development  is  in  place. 

C.         SHEPHERD  REAL-TIME  KERNEL  (SRK)  ARCHITECTURE 

The  SRK  includes  the  Timer  control,  Motion  control,  and  the  User  interface. 
Figure  4.4  below  illustrates  the  exact  architecture  of  the  motion  control  part  of  the 
system. 


bodyMotion 


joystick  x 


•> 


speedDot     >w 


omegaDot   ^ 


joySdcky 


Joy  Slick 
(physical  object) 


eomputeAclaalADglei 


4         + 


wheelMotion 


ctualAnRks 


ctuaiAnfikRatcs 


actuaK  peeds 


com  pate  Actual  Rates 

(takes  derivative) 


desiredS  peeds 


desiredAnaieRates 


dajpedAnflkg 


Steer  Readings 


DnveReadaiRS 


driveMotors 


SteerDtgas 


dmfblwr 


Speed  Digits 


dmcSseed 


Servo  Driven 
Moton  1 4 

StaTt  Encoders 


Servo  Driven 
Molen  1 4 

Shift  Encode n 


Read  Encoders 

_$ l_ 


re  adSte  e  r  Eh  code  rs 


readDrivcEa  coder 


Figure  4.4:  Shepherd  Motion  Control  Architecture 


1. 


Timer  Control 


The  timer  control  is  a  very  integral  part  of  the  system.  As  was  stated  earlier  SRK 
has  a  real-time  modifiable  timer  control.  This  timer  control  is  made  possible  through  the 
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use  of  the  Taurus  board's  AM9513A  Counter/Timer.  This  Timer/Counter  provides  five 
16-Bit  general  purpose  counters  and  uses  a  4-Mhz  oscillator  as  a  clock  input  with  the 
outputs  connected  to  the  Taurus  Interrupt  structure  for  processing.  The  Timer  5  group  of 
the  AM9513  device  was  utilized  due  to  it's  compatibility  with  the  68040  Processor  and 
contains  it's  own  timer  handler. 

The  main  routine  in  controlling  the  timing  and  setting  the  interrupts  is  located  in 
the  timer.c  file  along  with  the  header  file  in  timer.h  (Appendix  F).  This  timer  is 
initialized  and  started  in  the  main  shepherd  routine.  It  continuously  provides  a  10 
millisecond  interrupt  until  the  program  is  terminated.  However,  this  value  could  be 
modified.  This  was  made  possible  by  manipulating  the  data  port  values  of  the  AM95 1 3- 1 
device  and  multiplying  those  values  by  factors  of  10000,  1000,  100,  10,  or  1  to  obtain  a  1, 
0.1,  0.01,  0.001,  or  0.0001  second  interrupt  in  that  order.    The  accuracy  of  this  timing 
was  tested  using  an  oscilloscope  and  also  a  frequency  analyzer.  It  was  found  through 
testing  that  the  SRK  when  in  operation  only  utilizes  approximately  2.7  milliseconds  total 
in  handling  all  the  associated  routines.  For  further  information  on  this  counter/timer  see 
reference  2 1 . 


2.  Motion  Control 

Referring  back  to  Figure  4.4,  motion  control  encompasses  several  major  parts 
which  are  woven  into  a  tightly  controlled  structure  for  driving  and  Steering  Shepherd's 
wheels.  The  bodyMotion  function  takes  as  input  the  mode  of  motion  desired  from  the 
user.  Using  this  mode  and  the  necessary  instructions  programmed,  it  provides  5  inputs  to 
the  wheelMotion  function  which  are:  motion,  speedDot,  thetaDot,  omegaDot,  and 
vehicle.  Motion  is  a  structure  consisting  of  the  user's  input  of  speed,  theta  (direction  of 
travel  of  the  vehicle),  and  omega  (rotational  speed  of  the  vehicle).  Vehicle  is  a  structure 
consisting  of  the  x  and  y  coordinate  of  the  vehicle  on  some  x-y  plane  for  tracking 
purposes  and  the  heading  of  the  vehicle.  SpeedDot,  thetaDot,  and  omegaDot  all  are  a 
derivation  of  speed,  theta,  and  omega  over  time. 

The  wheelMotion  function  takes  the  given  inputs  along  with  inputs  from  a 
feedback  loop  for  the  actual  positions  of  the  encoders  for  both  driving  and  steering  of  the 
vehicle,  and  performs  calculations  based  on  the  theory  discussed  in  chapter  VI.  The 
results  of  these  calculations  are  then  sent  to  the  driveMotors  function  which  provide  these 
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values  to  the  servo  drivers  for  steering  and  driving  the  vehicle  wheels  resulting  in  the 
motion  of  Shepherd. 

3.  User  Interface 

The  user  interface  is  facilitated  through  the  use  of  a  Texas  Instruments  laptop 
computer  running  the  Windows  95  operating  system.  By  utilizing  the  HyperTerminal 
program  accessory  to  connect  the  laptop  to  the  unix  system  via  an  ethernet  connection, 
program  files  can  easily  be  receive  from  the  unix  system  after  being  compiled  and  the 
executable  files  sent  to  the  taurus  board.  After  successful  downloading  of  the  program  to 
the  Taurus  board,  the  user  can  then  control  the  operation  of  Shepherd  through  the  laptop 
keyboard.  Figure  4.5  illustrates  this  user  interface  after  the  program  is  successfully 
downloaded  and  ran.  An  on-screen  menu  is  displayed  with  a  description  of  Shepherd 
motion  that  can  be  initiated  via  keyboard  strokes. 

This  menu  is  located  in  the  file  user.c  (Appendix  C).  It  is  a  simple  character 
definition  which  is  called  from  the  user.c  file.  The  menu  choice  inputs  from  the  keyboard 
are  converted  to  ascii  characters  which  are  interpreted  in  the  user()  routine  switch 
statement  to  launch  the  associated  menu  item  seen  in  figure  4.5  on  the  next  page. 

Also,  part  of  the  user  interface  is  a  joystick.  This  joystick  interface  is  activated 
when  menu  choices  3  and  4  are  selected.  It  controls  Shepherd's  wheels  for  both  steering 
and  driving.  This  will  be  discussed  in  Chapter  VI. 
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V.  EXPERIMENTAL  RESULTS  ON  MOTION  CONTROL 


A. 


OVERVIEW 


Chapters  III  and  IV  of  this  thesis  discuss  how  each  servomotor  can  be  accessed 
and  voltages  applied.  However,  the  aforementioned  chapters  do  not  explain  how  the 
incremental  inputs  placed  on  the  DA  translate  to  wheel  speed  or  what  the  maximum  and 
minimum  wheel  speeds  are.  So,  how  were  the  maximum  and  minimum  wheel  speeds 
determined?  Moreover,  what  type  "controls"  are  required  to  ensure  that  each  wheel  has 
the  same  driving  velocity  or  angular  velocity?  We  used  a  modified  version  of  the 
"scientific  method"  to  establish  and  carry  out  the  experiments  for  the  Shepherd  vehicle. 

Scientific  Method  and  Approach 


New  Model 
or 

New  Theory 


ivtrapolate  or  Expand 
Theory /Mo  del 


Validated 

General 

Assumption  (s) 

Hypothesis 

Test  and 
Verify  Hypothesis 

~       1 

Modify  or  Chajvge 
Hypothesis 

l       t       i 

t 

information 
(input) 
-observations 
apriori  date 
-  Guesses 

Data 
-measurements 

observation 

L 

Rejected  Hypothesis 

Re-visit  Initial 
Assumptions 

and 
validate  Accuracy 

Figure  5.1:  Modified  version  of  the  scientific  method  and  approach  used  for  Shepherd  development. 
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As  with  any  experiment,  results  must  be  consistent  and  reproducible.  Considering 
the  vehicle  as  a  "closed  system"  observation  of  all  experimental  results  should  lead  to 
deduction,  connection,  or  correlation.  As  is  common  with  the  scientific  method  progress 
can  be  very  rapid  or  slow.  Sometimes  there  were  unexpected  results  (e.g.,  singularities), 
and  in  some  cases  our  dead-end  path  (failed  experiment  segment)  provided  some  very 
important  insights  which  helped  to  improve  vehicle  performance.  An  Ockham's  razor 
approach  was  taken  when  making  investigations  and  deductions — meaning  that  any 
unknown  phenomena  or  behavior  should  be  explained  in  terms  of  what  is  already  known 
(and  testing  the  simplest  possibility  first). 

B.         WHEEL  DRIVING 

Once  an  understanding  of  the  mechanics  and  providing  the  coded  structures  to 
move  the  wheels  had  been  achieved  the  concept  of  precise  and  controlled  driving  motion 
becomes  the  focus.  The  basic  initial  idea  was  to  measure  the  counts  from  the  counter 
board,  measure  wheel  revolutions  over  time.  Again  reiterating  a  modified  scientific 
method  was  used —  and  apriori  data  such  as  gear  ratio  and  other  engineering  data  were 
used  to  verify  results. 


1.  Developing  Driving  Constants 

The  digitToRadDrive  constant  [Appendix  J,  Consolidated  header  files,  line  364] 
was  the  first  constant  to  be  determined  for  wheel  driving.  The  biggest  problem  with 
developing  this  constant  was  working  around  the  10-millisecond  timer  interrupt  and  the 
lack  of  a  fully  functional  operating  system.  Since  Shepherd's  SRK  was  developed  from 
the  ground  up  within  the  last  year  it  has  limited  I/O  capabilities,  and  there  is  no  long-term 
storage  on  the  Taurus  board.  What  this  means  is  that  the  results  of  test  programs  would 
have  to  be  sent  to  a  monitor  (VT220)  or  to  a  printer.  We  decided  to  print  to  both  the 
monitor  and  printer.  However,  this  also  yielded  unexpected  results.  The  print  function 
used  too  much  of  the  10  milliseconds  to  allow  for  proper  functioning  of  the  process 
currently  running.    Hence,  we  moved  the  print  function  from  the  real-time  portion  of  the 
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code  to  alleviate  this  problem.    Moreover,  this  became  a  useful  technique  that  was  used 
throughout  Shepherd  development  and  testing. 

The  first  step  in  determining  the  number  of  counts  is  reading  the  counter  board. 
In  the  algorithm  used  in  the  SRK,  the  initial  values  on  the  counter  board  are  read,  stored, 
and  read  again  after  a  complete  revolution  of  the  tired  is  complete;  the  absolute  difference 
between  the  initial  counter  value  and  the  final  counter  is  the  actual  count  take  for  the 
revolution.  It  should  noted  however  that  consecutive  reads  of  the  counters  is  actually 
accomplished  and  the  values  adjusted  for  the  transition  from  Oxffffff  to  0x000000  and 
vice  versa  (24  bits)  [Appendix  E,  Motor.c,  lines  607-621].  An  example  of  the  code  to 
read  the  encoders  for  driving  can  be  seen  below: 

void  readDriveEncoders (unsigned  long  int  arrayQ) 

{ 

unsigned  char  *p=(unsigned  char*)VMECTRl,  cl,  c2,  c3; 

int  ix; 

long  int  temp; 

for  (ix=0;  ix<4;  ix++)  {     /*  read  all  four  motors  subsequentially  */ 

*(p+3)=0x03;  /*  load  output  latch  from  counter  */ 

*(p+3)=0x01;  /*  control  register,  initialize  two-bit 

output  latch  */ 
/*  read  three  bytes  for  specific  counter  ix  and  save  in  status    */ 
/*  first  access  to  Output  Latch  Register  reads  least  significant  */ 
/*  byte  first  */ 

cl  =  *(p+l)&0x00ff; 
c2  =  *(p+l)&0x00ff; 
c3  =  *(p+l)&0x00ff; 
array[ix]  =  ((unsigned  int)cl)  |  ((unsigned  int)c2  <<  8)  | 

((unsigned  int)c3  <<  16); 
p=p+4;  /*  increment  pointer  for  next  counter  */ 

} 
return; 
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}  /*  end  of  readDriveEncoders  */ 

Secondly,  a  method  to  display  the  enconder  data  is  required.  The  SioOut 
[Appendix  E:,  Motor.c,  lines  483-490  and  381-399]  display  routine  from  SRK  was  used 
for  this,  but  failed  because  the  time  required  to  display  the  data  exceeded  10  milliseconds. 
So,  we  moved  the  display  "call"  for  the  function  from  the  real-time  portion  of  the  code. 
In  SRK,  immediately  following  the  "driver"  routine's  call  the  following  type  routine 
would  be  placed  in  the  main  "user"  routine: 

while  (1) 

{ 

while(edCounter%200  !=  0){}; 

displayCount(); 

>; 

The  aforementioned  routine  would  print  based  on  the  edCounter  and  the  timer.  The 
counter  being  incremented  every  10  milliseconds.  Hence  the  value  is  displayed  every  2 
seconds  in  real-time.  The  key  is  the  fact  that  the  printing  is  being  executed  outside  of  the 
driver  routine. 

Now  all  the  elements  are  in  place  to  determine  the  digitToRadDrive  constant. 
After  testing  each  wheel  for  a  minimum  of  1000  revolutions  (called  double  pi  or  DPI 
below)  the  digitToRadDrive  constant  was  determined: 

#define    digitToRadDrive  -6.015495746e-5 

/*  driving  constant  rad/count  =  DPI/ 104450      May  8  */ 

/*  Experimental  Results  by  Ed  Mays  May  7  */ 

/*  Wheel  1  count  =  104456  */ 

/*  Wheel  2  count  =  1 04435  */ 

/*  Wheel  3  count  =  104454  */ 

/*  Wheel  4  count  =  104455  */ 

/*  Average  count  =  104450  */ 

/*cf.  2048*51  =  104448  */ 


36 


Verification  of  the  constant  was  made  possible  by  the  engineering  data.  Given  the  range 
of  inputs  (same  as  Figure  3.12)  to  the  Servomotors  and  the  gear  ratio  (section  3.B.3)  the 
digitToRadDrive  was  be  verified  above  (cf.  2048  *  51  =  104448).  The  value  of  51  vice 
50  for  the  gear  ratio  (was  determined  empirically  to  be  1:51). 

The  digitToCmDrive  was  much  more  trivial  to  determine.  The  digitToRadDrive 
provides  the  number  of  counts,  knowing  the  wheel  radius  (18.9cm),  and  using  circle 
circumference  formula  yields: 

#define  digitToCmDrive  0.0011369287 

/*  driving  constant  cm/count  =  digitToRadDrive*  18.9cm  Ed  Mays  */ 
/*  5/8/97  */ 

Now  having  the  "drive"  encoder  count  allows  the  computation  of  distance  traveled  (cm) 
by  the  vehicle;  and  coupled  with  the  timer  interrupt  allows  for  the  computation  of 
velocities  and  accelerations. 

2.         Measuring  Wheel  Speeds 

The  next  step  in  control  of  the  wheels  is  being  able  to  manipulate  wheel  speed. 
Developing  wheel  speed  control  was  one  of  the  projects  many  dead-end  path's — that 
eventually  lead  to  great  results.  There  were  three  problems  with  the  work  presented  here. 
First,  the  inputs  or  digits  were  not  applied  at  the  lowest  (hardware)  level  using 
SpeedDigits  [Appendix  E],  instead  the  value  was  considered  desiredSpeeds.  Secondly, 
the  input  values  were  massaged  before  being  used  to  ensure  that  all  the  system  hardware 
would  accept  the  range  of  values.  Thirdly,  the  uniqueness  of  each  driving  motor  and 
natural  output  variance  over  a  range  of  inputs  was  not  considered  (e.g.,  used  averages 
instead  of  individual  motor  data).  How  did  this  happen?  A  minor  communication  error 
and  poor  naming  of  arrays  made  this  possible.  However,  a  look  at  the  results  and  the 
logic  for  deriving  them  will  provide  insight  towards  the  actual  solution. 

Include  are  an  estimated  velocity  and  the  software-measured  (actual)  velocity  as 
determined  by  software.  The  estimated  velocity  was  determined  by  applying  the 
requisite  input  and  measuring  the  number  of  seconds  it  took  for  wheel  one  to  complete  10 
revolutions  (e.g.,  wheel  radius  18.9cm,  distance  traveled  =  2  *  PI  *  Revolutions  *  radius 
=  1187.5220  cm);  inaccuracies  from  this  measurement  came  from  hand  timing  and  the 
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version  of  pi  used  on  a  desk  calculator.   Velocity  calculated  from  V=distance  (cm)/time 
(seconds). 

The  software-measured  velocity  used  the  digitToCmDrive  constant,  which  has  the 
unit's  cm/count.  The  algorithm  subtracts  the  previous  count  from  the  present  counter  (as 
read  from  the  counter  board)  and  multiplies  the  result  by  digitToCmDrive  leaving  the 
outcome  with  the  unit  of  cm.  This  outcome  is  then  divided  by  .01  (DeltaT)  representing 
100th  of  a  second  or  10  milliseconds  (corresponding  to  the  system  interrupt).  The 
99999999  represents  a  value  not  representable  by  the  counter  board.  The  output 
displayed  to  the  monitor  every  100  calls  of  the  routine  (mod  100).  An  example  of  the 
code  to  compute  each  wheel's  driving  speed  can  be  seen  below: 

void  computeActualRatesO 

{ 

int  i; 
double  count,speed; 

for(i=0;  i<=3;  i++) 

{ 

if(PreviousCountSpeed[i]  ==  99999999)   /*  for  derivative  for  speed  */ 
Drive_Speed_Actual[i]  =  0.0; 
else 
Drive_Speed_Actual[i]  = 
(convertDifference((WheelDriveValues[i]  -  PreviousCountSpeedfi])) 
*DigitToCmDrive[i]) /DeltaT; 
PreviousCountSpeedfi]  =  WheelDriveValues[i]; 

} 
} 
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Input 
(digits) 

Time 
Stop  watch 

(sec) 

Estimated  Velocity 

(cm/s) 

Software 

Measured  Velocity 

(cm/s,  average) 

10 

116.27  sec 

10.21349 

10.23235 

20 

58.15 

20.42170 

20.46.471 

30 

39.10 

30.37141 

30.35599 

50 

No  data 

No  data 

50.70702 

60 

No  data 

No  data 

60.93937 

70 

No  data 

No  data 

70.94435 

80 

No  data 

No  data 

81.29040 

90 

No  data 

No  data 

87.65720 

Figure  5.2:  Inputs  and  results  from  massaged  data  (error).  No  data  entries  exist  because  the 
revolutions  were  too  fast  for  hand  timing. 

If  an  input  of  100  is  used  the  estimated  values  for  velocity  are  nolonger  linear.  If  the 
inputs  vs.  velocities  were  linear  the  ratio  20/20.421 70=  100/X  used  to  predict  the 
estimated  velocity  would  yield  a  velocity  of  102.32355  cm/s;  however,  the  software- 
measured  velocity  was  87.54350  on  average  for  all  input  values  from  100  to  1000  (Figure 
5.3). 


Input 
(digits) 

Time 
Stop  watch 

(sec) 

Estimated  Velocity 

(cm/s) 

Software 
Measured  Velocity 

(cm/s) 

100 

No  data 

No  data 

87.65720 

200 

No  data 

No  data 

87.65720 

400 

No  data 

No  data 

87.65720 

1000 

No  data 

No  data 

87.65720 

Figure  5.3:  Inputs  vs.  measured  velocity.    Figure  showing  a  velocity'  saturation. 


Hence,  the  relationship  between  the  input  and  the  velocity  look  somewhat  linear  until  the 
area  where  the  input  is  greater  than  70—  after  this  velocity  saturation  seems  to  occur  (or 
maximums  reached).    Moreover,  if  one  closely  looks  at  the  slope  (i.e.,  y  =  mx  +  b) 
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between  the  input  ranges  20-30,  83-84,  and  87-88  it  is  clear  that  the  slope  changes 
dramatically  over  these  regions.  Even  though  the  wrong  input  structure  and  mechanism 
was  used  to  generate  this  data  a  lot  was  learned.  For  instance,  motor  performance  is  not 
completely  linear  and  each  motor  has  somewhat  unique  characteristics.  Once  the 
aforementioned  problems  were  identified  and  corrected  the  correct  data  [Appendix  N] 
could  be  plotted.  Figure  5.4  is  a  plot  of  the  correct  data. 


Driving  Velocity  vs.  Input 


saturation 


100 


^      50 


o 

o 

I 

o 


o 
o 
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-50- 


•100 


saturation 


•1250  -1000  -750  -500  -250 


250   500   750   1000  1250 


Input  Values  (units) 

Figure  5.4:  Driving  Input  vs.  Velocity  plot.  Input  units  or  digits  are  written  directly  to  SpeedDigits. 
The  slope  changes  for  the  overall  graph  are  constant,  making  it  look  linear.  However,  over  some 
regions  this  graph  is  non-linear,  this  will  be  dealt  with  under  the  controls  section.  Velocities  also 
independently  verified  by  use  of  tachometer. 


3.  System  Controls 

So  far  controlling  the  speed  of  individual   servomotors  has  been   discussed. 
However,  it  is  known  individual  servomotors  provide  differing  outputs  for  the  same  input 
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ranges  in  some  cases.  Moreover  the  wheels  must  be  coordinated  and  work  together.  The 
goal  of  this  project  was  to  have  an  actual  driving  speed  that  has  less  than  1  %  error  when 
compared  to  the  desired  input  speed  (for  each  wheel).  How  is  this  close  tolerance 
accomplished?  This  small  margin  of  error  is  accomplished  by  using  well-established 
concepts  from  "control  systems"  theory.  A  general  control-system  structure  contains 
inputs  (or  reference  commands),  a  controller  (with  external  power),  control  forces,  a 
controlled  system  (plant),  disturbance  inputs,  outputs,  and  output  monitoring.  Control 
systems  are  almost  a  discipline  unto  themselves  requiring  knowledge  of  differential 
equations  and  Laplace  transforms.  Shepherd  is  looked  at  as  a  closed-loop  where  output 
monitoring  is  accomplished  through  sensors  (encoders)  and  the  information  passed 
through  feedback  channels.  The  feedback  results  in  a  closed  loop  signal  or  information 
flow.  The  controller  design  for  Shepherd  is  linear  and  considered  a  single-input-single- 
output  (SISO)  system.  Hence,  conceivably  the  state-variable  or  the  transfer-function 
(input-output)  method  could  be  used  here.  What  technique  did  we  use?  We  used  the  trial 
and  error  technique  [Ref.  17].  The  trial  and  error  technique  (Figure  5.5)  was  chosen 
because  of  the  skill  and  knowledge  levels  of  the  Shepherd  team.  The  advantages  of  this 
technique  are: 

1 .  Simple  mathematical  tools  are  used 

2.  Vast  amounts  of  experience  have  accumulated 

3.  Especially  well  adapted  for  use  with  computers 

4.  Linear  designs  usually  are  acceptable 

The  disadvantages  are: 

1 .  Inconsistent  performance  specifications  (PS)  can  be  encountered 

2.  Design  is  not  optimal 

3.  The  method  is  usually  suitable  mostly  for  SISO  systems. 
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Figure  5.5:  A  flow  graph  of  the  trial-and-error  design  process  [Ref.  18]. 

Using  the  experience  of  both  Professors  Kanayama  and  Yun  as  the  guide  the 
"black  box"  servo  structure  was  developed  (Figure  5.6).  The  servo  structure  is  called 
"black  box"  because  of  the  lack  of  understanding  of  the  servomotors  at  that  time 
(however,  inputs  and  outputs  could  be  measured).  The  previously  mentioned  PS  was  an 
error  rate  of  less  that  1%  of  the  given  reference  input.  There  were  other  PS's  governed 
by  heuristics.  For  instance,  it  was  desired  that  the  actual  output  velocity  converges  (in 
the  mathematical  sense)  on  the  reference  or  input  velocity,  with  near  perfect  static  follow- 
up. 
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Figure  5.6:  "black  box"  servo  structure  with  feedforward  and  feedback  compensation. 

Vibrations  and  other  unacceptable  behaviors  were  used  to  determine  if  a  smooth 
and  acceptable  convergence  had  been  achieved  for  the  gain  used  (e.g.,  if  the  robot  was 
shaking,  this  was  not  acceptable).  An  acceptable  gain  would  be  one  that  produces  an 
oscillatory  response  that  converged  quickly  to  the  reference  input  (Figure  5.7).  Again 
this  same  type  information  could  have  been  determined  by  a  better  scientific  guess  [Ref. 
19]  using  a  closed-loop  differential  equation. 

Response  as  a  Function  of 


\  Overdamped 
Response 


25 


50 

Time 


- 1 — 

75 


100 


Figure  5.7:   Proposed  "proper"  oscillatory  response  as  a  function  of  loop  gain.   Response  determined 
for  Shepherd  using  experience  and  heuristics. 
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Previously,  it  was  stated  that  the  input  vs.  velocity  plot  was  not  "truly"  linear. 
These  non-linear  ranges  have  severe  physical  realities.  Also,  mentioned  was  the  point 
that  each  servomotor  produces  an  output  that  may  not  be  the  exact  same  as  the  outputs  of 
the  other  system  servomotors  given  the  same  input.  And  the  stated  PS  requires  that 
servomotor  outputs  be  within  1  %  of  the  desired  input  or  input  speed.  This  translates  into 
several  challenges.  First,  using  the  feedforward  technique  constants  were  developed  to 
ensure  that  the  PS  of  1%  is  met.  The  following  algorithm  or  averaging  technique  was 
used  for  direct  testing  of  inputs: 

if  (vi  <  v  <  v2) 
d  =  d,  +  (d2  -  di)/(v2  -vi)  *  (v  -  Vl) 


The  v's  and  d's  above  are  the  same  as  the  velocities  and  inputs  in  figure  5.8. 


J 

V 

L 

'                                                                                         ^"      Inputs 
'                                                       3         1023 

Figure  5.8:   Illustration  of  averaging  technique  used  to  select  "good" 
input  ranges. 

Using  the   aforementioned  averaging  technique   the  constants   were   developed   that 
maintained  all  servomotor  outputs  within  l%  PS. 
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These  constants  are  called  K1-K4,  and  K6.  These  constants  represent  the 
feedforward  values  that  are  applied  to  each  servomotor  over  a  specified  range  of  desired 
speeds.  The  desired  speed  ranges  give  are  because  of  the  piecewise  continuity  chosen 
because  of  the  non-linearity's  in  the  input  vs.  velocity  plot  for  the  servomotors.  The  "K" 
constants  were  developed  with  no  load  on  the  system  (e.g.,  wheels  free  floating).  Listed 
below  are  the  constants  K1-K4,  and  K6  (speed  is  in  centimeters/second): 


K1[0]= 11.448 
Kl[l]= 11.500 
Kl  [2] =11.496 
Kl  [3] =12.375 


/*0<=speed<=5*/ 


K2[0]  =  11.500 
K2[l]=l  1.500 
K2[2]  =  11.644 
K2[3]  =  12.000 


/*5>speed<8*/ 


K3[0]  =  l  1.611 
K3[l]  =  11.585 
K3  [2]  =  11.686 
K3[3]  =  l  1.840 


/*8>=speed<20*/ 


K4[0]  =  11.711 
K4[l]=l  1.659 
K4[2]  =  11.705 
K4[3]  =  11.727 


/*20>=speed<=70*/ 


K6[0]=11.710 
K6[l]  =  l  1.700 
K6  [2]  =  11.700 
K6[3]=11.715 


/*70>speed<K5*/ 


Above  the  bracketed  values  are  array  element  numbers.    Element  [01  refers  to 
wheel  1,  motor  Ml;  element  [1]  refers  to  wheel  2,  motor  M2;  element  [2]  refers  to  wheel 
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3,  motor  M3;  and  element  [3]  refers  to  wheel  4,  motor  M4.  A  constant  K5  was  also 
defined  as  87.4  cm/sec,  and  used  a  safety  cutoff  for  the  maximum  speed.  Figure  5.9 
shows  the  values  for  a  few  selected  desired  or  commanded  speeds  (most  are  within  the 
require  1%  error  in  PS). 


Desired  Speed 

Ml  Speed 

M2  Speed 

M3  Speed 

M4  Speed 

10 

10.20 

10.08 

10.10 

9.89 

20 

20.12 

20.11 

20.11 

19.93 

30 

30.03 

30.00 

30.02 

29.98 

40 

39.98 

39.91 

39.98 

39.98 

50 

49.98 

49.79 

50.08 

50.00 

60 

60.01 

59.77 

60.02 

60.02 

70 

69.91 

69.78 

69.95 

69.97 

Figure  5.9:  Desired  (commanded)  speeds  vs.  actual  "free  floating"  motor  speed.  The  application  of 
feedback  is  expected  to  move  the  speed  of  Ml  into  the  required  1%  error  for  the  PS. 

At  this  point  all  the  tools  and  techniques  (i.e.,  trial-and-error,  scientific  method, 
and  the  experience  of  Professor's  Kanayama  and  Yun)  are  in  place  for  the  application  of 
system  controls.  How  are  the  system  controls  (Figure  5.6)  implemented?  First,  the 
algorithm  is  presented: 

SpeedDigit  =  velocityReferenceTable(Omega_Speed,ix)  + 

DriveFeedBackGain*(Omega_Speed  -  Drive_Speed_Actual[ix]); 

SpeedDigit  represents  the  actual  digit  value  being  applied  to  the  servomotor.  In  the 
feedforward  part  of  the  loop  (Figure  5.6)  the  commanded  velocity  is  multiplied  with  a 
constant  Kl,  which  corresponds  to  the  "K"  constants  described  earlier  in  this  section?  In 
the  code  segment  above  the  function  "velocityReferenceTable"  is  called  (prior  to  the 
addition  of  DriveFeedBackGain*(Omega_Speed  -  Drive_Speed_Actual[ix])  ).  The 
"velocityReferenceTable"  applies  the  proper  "K"  constant  for  the  range  the  commanded 
speed  (Omega_Speed)  falls  within. 
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A  detailed  look  at  velocityReferenceTable  is  provided  below: 

double  velocityReferenceTable(double  desiredVelocity,int  i) 

{ 
double  inVelocity, 

outVelocity; 

inVelocity=new_abs(desiredVelocity); 

if  (inVelocity>=0.0  &&  inVelocity<=5.0) 
outVelocity  =  inVelocity*Kl[i]; 

if  (inVelocity>5.0  &&  inVelocity<  8.0) 
outVelocity  =  inVelocity*  K2[i]; 

if  (inVelocity>=8.0  &&  inVelocity<20.0) 
outVelocity  =  inVelocity*K3[i]; 

if  (inVelocity>=20.0  &&  inVelocity<=  70.0) 
outVelocity  =  inVelocity*  K4[i]; 

if  (inVelocity>70.0  &&  inVelocity<K5) 
outVelocity  =  inVelocity*  K6[i]; 

if  (inVelocity>  K5) 
outVelocity=1023; 

if  (desiredVelocity<  0.0) 
outVelocity  =  -  outVelocity; 

return  outVelocity; 
}  /*  end  velociryLookupTable  */ 
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It  should  be  noted  that  if  the  in  Velocity  is  greater  than  K5,  then  the  out  Velocity  is  given  a 
value  of  1023 — this  ensures  there  are  no  system  resets  because  the  input  digits  are  too 
large.  All  other  velocities  are  multiplied  by  a  specific  "K"  and  the  value  returned.  In  the 
feedback  part  of  the  loop  (Figure  5.6)  the  difference  between  the  commanded  velocity 
and  the  actual  velocity  is  multiplied  with  a  constant  DriveFeedBackGain  (also  K2  in 
Figure       5.6)        [Appendix       E,        lines       46        &       47].  Again        this 

(DriveFeedBackGain*(Omega_Speed  -  Drive_Speed_Actual[ix]) )  is  added  back  into  the 
inputs  used  for  the  next  time  the  process  is  run  (based  on  the  10  millisecond  timer 
interrupt). 

Hence,  now  the  final  key  to  this  control  system  would  be  finding  a 
DriveFeedBackGain  constant  that  would  provide  the  desired  "proper"  oscillatory 
response  (Figure  5.7)  and  ensure  the  servomotor  output  velocity  is  within  the  1%  error  of 
the  commanded  velocity  required  by  the  PS.  Using  Occam's  Razor,  the  trial-and-error 
flow  diagram  (Figure  5.5),  the  iterative  approach  in  the  scientific  method  (Figure  5.1), 
and  the  heuristics  provided  by  Professor's  Kanayama  and  Yun,  the  search  for  the  best 
DriveFeedBackGain  was  initiated.  The  heuristics  used  were  observation  based.  First, 
the  "gain"  used  could  not  cause  the  vehicle  to  shake  in  any  visible  manner.  Secondly,  the 
"gain"  used  had  to  quickly  move  the  actual  servomotor  speed  to  the  commanded  speed  if 
there  was  a  difference.  Pseudo  random  values  were  chosen  as  gains,  based  on  the 
experience  of  the  aforementioned  professors.  The  real  number  range  [-1.0,  1.0]  was  used 
to  test  the  gains.  On  the  negative  end  of  the  range  the  gain  was  incremented  by  +.05, 
until  the  gain  equaled  zero — the  results  were  not  acceptable.  On  the  positive  end  of  the 
range  the  gain  was  decremented  by  +.05,  until  the  gain  equaled  zero — at  .8  the  gain 
showed  the  best  results  (i.e.,  range  [0.0,  1.0]).  The  gain  was  defined  as 
DriveFeedBackGain  =  .8  [Appendix  J,  Consolidated  header  files,  line  389].  Hence  the 
gain  met  all  the  criteria  for  an  acceptable  design  (Figure  5.5)  and  validates  the  "black 
box"  servo  structure  as  envision  by  professors  Kanayama  and  Yun  (Figure  5.6). 
Moreover,  the  experiment  can  be  considered  a  multidisciplinary  success  between  physics, 
electrical  engineering,  and  computer  science. 
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c. 


WHEEL  STEERING 


As  with  wheel  driving,  the  same  approach  to  wheel  steering  was  used  in 
measuring  the  counts  from  the  counter  board  and  measuring  the  rate  of  turning  of  each 
wheel  over  time.  The  goal  was  to  observe  and  measure  each  wheels  turn  rate  and  create  a 
feedback  compensation  loop  as  in  wheel  driving  to  ensure  that  the  PS  of  1%  is  met. 


1.         Developing  Steering  Constants 

The  steering  constants  were  developed  in  the  same  way  as  the  driving  constants. 
The  counter  board  was  read  for  steering  values  and  then  displayed  .  A  digitToRadSteer 
(input  digits  per  radians  for  steering)  constant  value  of  -6.817692391e-5  (rad/count  = 
(2*pi)/(2048*45))  and  RadRateTodigit  constant  value  of  195.4155  (digits/rad/sec  = 
1023/5.23598)  was  determined  by  observation  of  the  data  forthcoming. 


2.  Measuring  Wheel  Rate  of  Turn 

In  measuring  the  wheel  rate  of  turn,  the  same  approach  was  taken  as  discussed 
before.  An  estimated  rate  of  turn  and  the  software-measured  (actual)  rate  of  turn  are 
included.  The  estimated  rate  was  also  determined  by  applying  an  input  and  measuring 
the  number  of  seconds  it  took  for  a  wheel  to  completely  rotate  360  degrees.  As  can  be 
deduced,  a  certain  amount  of  error  was  introduced  due  to  human  timing  interaction. 


Desired  rate  of 
turn 

(rad/s) 

Time 
Stop  watch 

(sec) 

Estimated  Rate 

(rad/s) 

Software 
Measured  Rate 
(rad/s,  average) 

1 

6.0 

1.00000 

0.98174 

2 

3.5 

1.79485 

1.95667 

3 

2.19 

2.86849 

2.93160 

5 

1.69 

2.71716 

3.90653 

5.5 

No  data 

No  data 

4.88828 
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Desired  rate  of 
turn 

(rad/s) 

Time 
Stop  watch 

(sec) 

Estimated  Rate 

(rad/s) 

Software 
Measured  Rate 

(rad/s,  average) 

10 

No  data 

No  data 

5.23598 

20 

No  data 

No  data 

5.23598 

30 

No  data 

No  data 

5.23598 

Figure  5.10:     Inputs  and  results  from  massaged  data  (error).     No  data  entries  exist  because  the 
revolutions  were  too  fast  for  hand  timing. 

By  observing  the  data  from  the  above  figure  we  can  confirm  that  a  certain  amount 
of  error  is  inherent  in  the  system.  As  the  input  value  increases  from  0  to  5.5  radians  per 
second  ,  a  linear  correspondence  tended  to  exist.  However,  after  an  input  of  5.5  rad/s  the 
software  measured  average  tended  to  be  5.23598  rad/s  resulting  in  a  saturation  state.  This 
result  in  itself  could  not  give  us  the  exact  range  of  values  where  this  occurred.  Therefore, 
manipulation  of  the  steerDigits  was  necessary.  Appendix  O  contains  these  results  given 
in  inputs  of  digits  vice  desired  rate  of  turn.  At  the  maximum  input  speedDigit  of  1023 
saturation  is  reached.  The  wheels  will  only  turn  at  an  average  rate  of  5.235  radians  per 
second.  Thus  the  software  measured  average  of  5.23598  rad/s  was  adopted. 


3.  Steering  Feedback 

By  further  observation  of  the  data  obtained,  an  average  was  used  in  obtaining  the 
rate  of  turn.  Not  all  the  wheels  turn  at  the  same  rate.  The  objective  was  to  have  a  less 
than  1%  PS  for  optimization.  To  achieve  that  another  "black  box"  servo  structure  was 
developed  .  The  Figure  5.11  is  a  pictorial  representation  of  this  and  is  a  little  different 
than  that  of  wheel  driving. 
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Figure  5.11:  "black  box"  servo  structure  with  series  and  feedback  compensation  for  wheel  steering 
feedback  control. 


In  finding  the  K  constants  from  the  figure,  trial  and  error  is  also  used.  The 
representation  of  the  algorithm  in  code  is  presented: 

Steer_Digit  =  rateReferenceTable(desiredAngleRates[ix]) 

+  steerFeedbackGain*(desiredA.ngleRates[ix]-actaalAngleRates[ix]) 
+  angleFeedbackGain*norm(desiredAngles[ix]-actualAngles[ix]); 

SteerJDigit  represents  the  actual  digit  value  being  applied  to  the  servomotor.  The 
rateReferenceTable  function  simply  converts  the  inputed  rate  to  digits  or,  in  the  case 
where  the  inputed  rate  is  larger  than  5.235,  clips  it  to  within  limits.  The  function  is  given 
below: 

double  rateReferenceTable(double  desiredRate) 

{ 

double  inRate, 

outDigit; 

inRate=new_abs(desiredRate); 

if  (inRate<=  5.234) 

outDigit  =  inRate*195.4155  ; 
else 

outDigit=1023; 
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if  (desiredRate<  0.0) 
outDigit  =  -  outDigit; 

return  outDigit; 


The  two  K  constants  steerFeedbackGain  and  angleFeedbackGain  were  determined 
by  trial  and  error.  AngleFeedbackGain  was  first  maintained  at  0.0  while  testing 
steerFeedbackGain.  SteerFeedbackGain  was  made  very  low  at  the  outset  and  increased 
through  each  test  until  the  vehicle  displayed  unusual  behavior  such  as  shaking  while 
operating  its  steering  function.  At  this  point  the  value  was  lowered  and  then  the  value  for 
angleFeedbackGain  was  increased  in  the  same  way.  These  values  became  optimal  at 
100.0  and  1000.0  for  steerFeedbackGain  and  angleFeedbackGain  respectively. 


4.  Wheel  Testing 

It  was  discovered  while  operating  the  vehicle  that  wheel  4  would  on  occasion  be 
very  badly  misaligned  from  the  other  3  wheels.  Even  after  repeated  realignment  it  would 
not  operate  as  the  others.  At  suspect  was  the  thought  this  problem  was  software  related  . 
To  test  this,  a  routine  was  inserted  into  the  SRK  driveMotors()  function  with  a  menu  item 
on  the  user  interface.  This  routine  simply  turned  wheel  4  360  degrees  in  one  direction 
until  the  wheel  aligned  read  the  encoders  for  angle  position,  paused  one  second  and  then 
turned  it  in  the  opposite  direction.  At  each  pause,  the  wheel  position  was  displayed  to  the 
interface  screen  and  recorded.  The  data  obtained  is  presented  below  in  Figure  5.12  for  10 
iterations  for  clockwise  and  counterclockwise  rotation: 


Clockwise  Rotation 

Counterclockwise  Rotation 

1 

000.867 

360.390 

2 

000.878 

360.390 

3 

000.976 

360.363 

4 

000.933 

360.414 

5 

000.984 

360.371 

6 

000.992 

360.394 

7 

000.992 

360.453 
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Clockwise  Rotation 

Counterclockwise  Rotation 

8 

000.902 

360.394 

9 

000.996 

360.445 

10 

000.996 

360.476 

Figure  5.12:  Wheel  4  data  based  on  position  of  rest  after  direction  of  turn. 

The  average  values  for  rotation  in  both  the  clockwise  and  counter  clockwise  directions 
were  000.955  and  360.426.  These  were  averaged  for  20  iterations  see  Appendix  O  for 
the  full  data  set.  Even  though  this  data  was  obtained  while  the  wheel  was  in  a  free 
floating  environment  without  added  friction,  it  still  proved  that  wheel  4  was  operating 
within  1%  PS.  Therefore,  from  this  result  a  conclusion  is  drawn  that  the  problem  is  not 
software  related  in  nature  but  maybe  mechanical. 
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VI.  MOTION  MODES 

A.         OVERVIEW 

Chapter  II  mentioned  several  modes  of  motions  that  can  be  exhibited  by 
Shepherd.  Due  to  the  nature  of  Shepherd's  characteristics  of  having  four  wheels  that  can 
be  independently  operated  with  two  degrees  of  freedom,  this  makes  it  possible  to  obtain 
three  degrees  of  freedom  motion.  In  this  chapter  we  will  discuss  the  modes  of  "Tornado" 
(complex  motion),  Joystick  controlled  motion  and  searching  motion  simulation.  The 
other  motion  modes  are  encompassed  by  these  motions.  The  code  for  the  other  motions 
are  provided  in  the  Appendices.  Also,  the  Tangential  mode  will  not  be  discussed  but  will 
be  left  for  a  future  reasearch  topic  and  implementation.  However  the  ground  work  as 
well  as  the  code  is  in  place  for  implementation  (Appendix  D). 

Before  embarking  on  this  discussion  on  motion  modes  the  theory  or  basis  for 
motion  control  must  be  presented  This  control  motion  theory,  as  well  as  the  figures  to 
follow,  was  taken  from  the  works  of  Professors  Yutaka  Kanayama  and  Xiaoping  Yun 
[refs  23  &  24].  First  a  vehicle  coordinate  system  is  defined  on  a  rigid  body  robot.  A 
configuration  q  is  defined  as 

(p,  \|/)  =  ((x,  y),  y), 

where  p  is  the  positioning  of  the  vehicle  origin  and  psi  is  the  heading  orientation  of  the 
vehicle  Xv-axis.  Next  in  describing  the  motion  of  the  vehicle's  configuration  which  is  a 
function  of  time,  the  following  is  the  definition: 

q(t)  ^  (p(t),  V|/(t))  =  ((x(t),  y(t)),  \|/(t)), 

where  p(t)  is  the  translational  component  and  \|/(t)  is  the  rotational  component  of  the 
vehicle  motion.  Figure  6.1  is  an  illustration  of  this  configuration  and  motion.  Because 
the  vehicle  possesses  2  dimensional  positioning  it  can  exhibit     3  degrees  of  freedom 
motion.     This  motion  contains  three  variables  of  : 

•     Translation  speed  --  v(t)  =  V  ((dx(t)/dt)2  +  (dy(t)/dt)2), 
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•  Motion  direction  --  6(t)  =  atan2(dy(t)/dt,  dx(t)/dt),  if  v(t)  >  0, 

•  Rotational  speed  --  co(t)  =  d\|/(t)/dt. 


Resulting  in  a  motion  description  of: 


Q(t)  =  (v(t),  0(t),  C0(t)). 


(I) 
Configuration 


y 


(II) 
Motion 


O 
Y 


O 


x 


Figure  6.1:  Configuration  and  Motion  of  a  Rigid  Body  [Ref.  23]. 

As  was  mentioned  in  Chapter  II,  3  typical  vehicle  motions  are  possible.  The  3 
typical  motions  are  illustrated  in  Figure  6.2.  Part  (a)  represents  an  all  too  familiar  motion 
exhibited  by  a  normal  automobile  or  bicycle  and  is  referred  to  as  tangential  motion.  This 
is  characterized  by  the  fact  that  the  vehicles  heading  orientation  is  equal  to  its 
translational  motion  direction  (  d\|/(t)/dt  =  6(t)/d(t)).  Part  (b)  depicts  a  motion  called 
constant  orientation  where  the  vehicles  heading  orientation  is  constant  or  rotational  speed 
of  the  vehicle  is  0.  And  part  (c)  shows  a  complex  motion  which  incorporates  rotation 
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Figure  6.2:  Typical  vehicle  motions  [Ref.  23]. 

independently  superimposed  on  a  translational  motion.  This  complex  motion  is  the  basis 
for  the  "Tornado"  motion  mode  to  be  discussed. 


B. 


"TORNADO"  MOTION 


In  analyzing  this  motion  mode,  several  more  discriptions  will  have  to  be  made 
clear.  One  is  that  of  a  point.  A  point  is  defined  as 

Pi  =  (x,,y,)*(0,0) 

described  in  the  vehicle  coordinate  system.  On  the  rotary  vehicle  it  corresponds  to  a 
wheel  .  In  the  case  of  wheels  1-4,  it  would  be  (40,-40),  (40,40),  (-40,  -40),  and  (-40,40). 
So  we  will  have  to  evaluate  how  these  wheels  move  while  the  vehicle  is  executing  the 
input  motion  or  Q.     In  order  to  evaluate  this,  the  polar  coordinate  representation     is 
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another  description  that  is  needed.     This  representation  is  defined  as  (p,  a)  and  is 
represented  as 

p  =  V(xi2  +yi2)  and  a  =  atan2(yi,  xi). 


The  subscript  is  a  representation  of  the  wheel  number  and  can  represent  any  wheel  based 
on  the  wheel  location  in  the  vehicle  coordinate  system.  Figure  6.3  is  a  representation  of 
the  composite  motion  of  a  point  on  a  vehicle. 


e 


i, 


LJ 


Figure  6.3:  Composite  Motion  of  a  Point  on  a  Vehicle  [Ref.  23]. 
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Given  the  above,  a  current  configuration  and  motion  of 

q(t)  =  ((x(t),  y(t)),  \|/(t)), 
Q(t)  =  (v(t),  9(t),  co(t)), 

in  the  global  coordinate  system,  the  x  and  y-components  of  Vix(t)  and  Viy(t)  in  the  global 
coordinate  system  can  be  determined  mathematically  [Ref.  23  p.3].  The  motion  speed 
vi(t)  and  direction  6i(t)  in  the  global  coordinate  system,  motion  direction  0iV(t)  in  the 
vehicle  coordinate  system,  and  rotation  rate  C0jV  of  pi  or  any  wheel  is: 

vi(t)  =  V((vlx(t))2  +  (vly(t))2) 
e,(t)  =  atan2(vly(t),  vlx(t)) 

e,v(t)  =  e,(t)-i|/(t) 

G),v  =  ((v2£+p2G33)+vpco((O+-0-)sin(6-\j/  a)+p(vD-<*co)cos((9-\}/-a))/v12  -  co 
■0-  is  theta  dot,  D  is  omega  dot,  and  ♦  is  motion  speed  dot. 

Corresponding  to  the  above  equations  in  SRK  is  the  following  code  from  Appendix  D 
(movement.c): 

ro=whp[i].rho; 

ro2=ro*ro; 

beta= vehicle.  heading+whp[i].  alpha; 

vlx  =  speed*cos(theta)-(whp[i].rho*omega*sin(beta)); 

vly  =  speed*sin(theta)  +  (whp[i].rho*omega*cos(beta)); 

desiredSpeeds[i]  =  new_sqrt(vlx*vlx  +  vly*vly); 

if  (new_abs(desiredSpeeds[i])  >  0.01)  { 

desiredAngles[i]  =  atan2(vly,vlx)  -  vehicle. heading; 

wheelAngleV  =  morion.Theta  -  vehicle. heading  -  whp[i]. alpha; 

desiredAngleRatesfi]  =  ( (speed*speed*thetaDot  +  ro2*Omega3) 
+spccd*ro*omega*(omega+thetaDot)*sin(wheelAngleV) 
+ro*(omegaDot*speed-omega*speedDot)*cos(wheelAngle\r) ) 
/(  desiredSpeedsfi]*  desiredSpeeds[i])  -  omega; 
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desiredAnglesO[i]  =  desiredAngles[i]; 
desiredAngleRatesO[i]  =  desiredAngleRatesfi]; 

} 

The  above  code  is  very  straight  forward.  A  direct  correlation  can  be  discerned 
from  the  theory  to  the  implementation.  The  variables  were  named  as  closely  as  possible 
to  match  the  theory  presented.  The  resulting  code  provided  the  mathematical 
computation  providing  the  resulting  values  from  a  rotation  superimposed  independently 
on  a  translation  motion.  It  is  not  shown,  but  all  variables  with  brakets  enclosing  an  'i' 
represent  a  wheel.  The  entire  routine  is  enclosed  in  a  for  loop  which  is  iterated  four 
times.  Therefore,  a  resulting  value  is  computed  and  provide  to  each  wheel's  servo  for 
both  driving  and  steering  . 

A  detailed  proof  of  the  rotation  rate  of  the  moving  direction  at  a  point  can  be 
found  in  Ref.  24.  As  was  stated  earlier,  this  motion  control  theory  is  from  the  work  of 
Professors  Kanayama  and  Yun.  The  authors  simply  implemented  this  theory  in  code  and 
applied  it  to  the  rotary  vehicle. 

C.        JOYSTICK  CONTROLLED  MOTION 

As  discussed  in  chapter  IV  the  user  interface  is  provided  by  a  laptop  that  includes 
a  selection  menu  of  shepherd  functions.  There  are  many  options  (still  expanding,  a  work 
in  progress)  on  the  menu.  The  two  of  concern  here  are  options  three  (3)  and  four  (4), 
straight  motion  by  joystick  and  XY-motion  by  joystick.  Actually  the  emphasis  will  be  on 
option  four  because  option  three  can  be  considered  a  logical  subset  of  option  four. 

The  "driver"  function  discussed  in  the  file  movement.c  is  called  every  10 
milliseconds.  One  of  the  key  functions  executed  under  driver  is  the  call  to  another 
function  named  "bodyMotion".  The  bodyMotion  function  is  also  located  in  movement.c. 
When  a  user  selects  option  three  or  four  from  the  menu  a  motion  mode  3  or  4  is  chosen. 
If  the  user  chooses  option  four  the  motion  mode  is  4.  The  case  the  user  chose  causes  the 
joystick  to  be  read  by  the  readJoyStick  function  (Appendix  H,  utils.c).  This  function 
reads  the  three  ports  (A,  B  and  C)  from  the  Intel  85C55  Parallel  Port  1  (Taurus  board) 
and  converts  them  into  an  ASCII  string  (code  segment  below): 
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void  readjoyStick(void) 

{ 

unsigned  int  i,index; 

unsigned  char  *ctriPort=  (unsigned  char*)PI01_CTRL; 
unsigned  char  *dataPort=  (unsigned  char*) PI01_D ATA; 
unsigned  int  pioPortl  [3]; 
double    z=  0.1,  xx,  yy,  zz; 

*ctrlPort=0x9b;    /*  set  all  ports  (A,B,C)  into  input  mode  (read  only)  */ 
index=10;  /*  position  for  x-digits  in  string  JOYSTICK  */ 

for  (i=0;i<3;i++) 
pioPortl  [i]  =  *(dataPort+i); 

xx  =  (double)pioPortl[0]-128.0; 
yy  =  (double)pioPortl[l]-128.0; 
if(xx>=  0.0) 

xx  =  xx*xx/100; 
else 

xx  =  -xx*xx/100; 
if  (yy  >=0.0) 

yy  =  yy*yy/100; 

else 

yy  -  -yy*yy/l00; 

joyStick.x  =  a*(xx)  +  (1.0-a)*joyStick.x; 
joyStick.y  =  a*(yy)  +  (1.0-a)*joyStick.y; 

if  (pioPortl  [2]==0x03) 

setVME((unsigned  char  *)VME92 10,0x00);  /*  no  button  pressed      */ 
else  { 
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setVME((unsigned  char  *)VME92 10,0x02);  /*  if  any  button  pressed  */ 

} 
} 

It  should  be  noted  that  the  joystick  input  integer  range  is  from  [-127,128];  the 
intersection  of  the  'x'  and  'y'  axis  on  the  physical  joystick  defines  the  center  (x=0,  y=0). 
Once  the  port  is  read  some  data  smoothing  is  done.  Due  to  the  sensitive  nature  of  the 
inputs  a  parabolic  function  was  added  for  control  (this  can  be  seen  above  with  the 
manipulation  of  the  xx  and  yy  variable).  The  purpose  of  the  parabolic  function  is  to 
ensure  that  when  the  joystick  input  values  are  small  (near  zero,  center  on  the  physical 
joystick)  the  slope  changes  will  be  of  minimal  effect,  however  if  the  input  values  are 
large  (away  from  the  physical  joystick  center)  the  effect  on  velocity  or  steering  will  also 
be  proportionally  large.  The  smoothing  is  continued  for  because  of  the  possibility  of  very 
quick  slope  changes  in  the  data  being  read-in.  The  objects  joyStick.x  and  joyStick.y 
receive  values  that  are  only  10  percent  (a  =  0.1  in  the  code  segment)  of  the  xx  or  yy  value 
plus  90  percent  of  the  previous  value  for  xx  or  yy.  The  aforementioned  smoothing 
techniques  were  developed  based  on  the  experiences  of  Professor  Kanayama  an  the 
constant  "a  =  0.1"  determined  by  testing  for  the  "best"  hand  feel  and  response. 

Upon  completion  of  the  read  and  smoothing  of  the  joystick  data,  these  values  (i.e., 
speed  and  theta)  are  passed  to  the  wheelMotion  function  described  in  the  Shepherd 
Motion  Control  Architecture  (see  code  segment  below): 

case  4:     /*  X-Y  Motion  by  Joystick  */ 

readJoyStick();  /*  ejm  19  July  97*/ 

speed  =  -joyStick.y*0.1;      /*  speed  control,  0.1  determined  by  testing  */ 

theta  =  -joyStick. x*0. 02;    /*  steering  control,  0.02  determined  by  testing  */ 

if  (theta  >   HPI)  theta  =   HP1; 

if  (theta  <  -HPI)  theta  =  -HPI; 

/*  omega  =  -joyStick.omega*0.1;*/   /*    pending  ejm  24  July  97  */ 

break; 
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The  two  "if  a  statements  with  the  theta  conditions  above  reflect  the  capability  of 
the  rotary  vehicle  to  complete  perpendicular  driving  and  parking.  Actually,  the  theta 
values  (steering  angle)  of  the  Shepherd  vehicle  are  unlimited,  however  they  are 
constrained  here  for  ease  of  use  and  control. 

D.         SEARCHING  MOTION 

The  searching  motion  discussed  here  is  based  on  the  requirement  to  have  a 
smooth  technique  that  allows  shepherd  to  evenly  and  precisely  search  an  area  for  UXOs. 
The  aforementioned  search  algorithm  and  its  implementation  are  not  trivial.  The 
algorithm  and  simulation  presented  here  are  the  results  of  a  lifetime  of  work  by  Professor 
Kanayama.  Professor  Kanayama's  expertise  in  the  areas  of  motion  planning,  motion 
design,  vehicle  kinematics,  sensing,  guidance,  learning,  environmental  representation, 
and  control  architectures  for  autonomous  vehicles  was  the  major  influence.  Professor 
Kanayama's  work  on  the  Yamabico-11  robot  includes  development  of  composite 
function,  line  tracking,  circle  tracking,  and  neutral  switching  technique  [Ref.  7].  For  the 
aforementioned  search  algorithm  and  simulation  the  composite  function  and  line  tracking 
technique  will  be  used. 

The  goal  of  this  simulation  is  to  show  that  if  given  an  orientation  for  the  vehicle 
body  and  a  given  path,  that  the  path  can  be  tracked  smoothly  and  the  vehicle  orientation 
will  also  change  to  ensure  area  coverage  of  the  path  traveled.  Why  is  this  important? 
This  is  critical  because  the  desire  is  for  the  vehicle  to  search  the  path  in  the  most  safe, 
smooth,  and  efficient  manner.  The  first  assumption  is  that  the  time  required  to  move 
across  a  path  is  10  milliseconds  or  0.01  seconds.  Secondly,  an  assumption  can  be  made 
for  the  vehicle  orientation  (called  psi  here),  psi  starts  at  3*PI/4.0.  Along  the  path  traveled 
the  orientation  or  psi  will  move  from  3*PI/4.0  to  PI/4.0  (having  a  net  change  of  PI/2.0). 
From  the  aforementioned  change  in  psi,  the  incremental  change  over  time  can  be 
determined  (this  incremental  change  is  called  omega).  Dividing  the  net  difference  in  psi 
(Pi/2.0)  derived  omega  by  10  milliseconds,  resulting  in  an  omega  value  of  0.1570796327. 
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A  value  of  40  centimeters  per  second  was  arbitrarily  chosen  for  the  vehicle  velocity.  For 
the  simulation  the  initial  vehicle  body  coordinates  as  x  =  0,  y  =  0,  and  the  vehicle 
orientation  as  shown  above  3*PI/4.0.  Also,  the  coordinates  of  the  wheels  must  be  known. 
Hence,  we  place  the  vehicle  wheels  on  sides  that  are  80  centimeters  in  length  (like  the 
Shepherd  vehicle). 
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The  last  item  is  the  structure  required  supporting  the  simulation  [Ref.  M],  below 
is  a  code  segment  to  illustrate  the  aforementioned  statements. 

double  deltaTime  =  0.01; 

double  Vel  =  40.0; 

double  omega  =  -0.1570796327; 

typedef  struct  { 

double  x; 

double  y;    } 
POINT; 

typedef  struct  { 
POINT  Point; 
double  Theta; 
double  Kappa; 
double  Psi; 

} 
CONFIGURATION; 

q_init.Point.x  =  0.0; 

q_init.Point.y  =  0.0; 

q_init.Theta     =  0.0; 

q_init.  Kappa    =  0.0; 

q_init.Psi    =  2.356219449;  /*  3*PI/4.0  */ 
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//individual  wheel  coordinates 
qfrontR.Point.x  =  40;    /*  wheell  */ 
qfrontR.Point.y  =  -40; 

qfrontL.Point.x  =  40;    /*  wheel2  */ 
qfrontL.Point.y  =  40; 

qrearR.Point.x  =  -40;    /*  wheeB  */ 
qrearR.Point.y  =  -40; 

qrearL.Point.x  =  -40;    /*  wheel  4  */ 
qrearL.Point.y  =  40; 


q_xaxis.Point.x  =  0.0;  /*  line  to  be  tracked,  initial  configuration  */ 

q_xaxis.Point.y  =  40.0; 

q_xaxis.Theta  =  0.0; 
q_xaxis. Kappa      =  0.0; 

Another  key  element  in  the  structure  CONFIGURATION  is  theta.  Theta  is 
simply  the  angle  to  the  path  being  tracked.  For  instance  if  the  vehicle  is  tracking  a  line, 
when  the  vehicle  move  onto  the  actual  line  then  Theta' s  value  goes  to  zero.  The  final 
element  required  for  the  simulation  is  the  step  size  that  is  used  for  the  motion.  The  step 
size  in  the  simulation  code  is  called  deltaS,  and  is  vel*deltaTime  (or  .4  centimeters  per 
second).  Armed  with  this  knowledge  a  simplified  discussion  can  take  place  (for  detailed 
knowledge  of  the  compose  function,  and  line  tracking  see  Professor  Kanayama's  motion 
planning  and  kinematics  notes  [Ref.  7]). 
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The  compose  function  is  used  to  determine  (using  deltaS  )  the  next  position  of  the 
vehicle  body.  Here  two  compose  functions  are  being  used,  one  that  is  for  the  wheels 
(Compose2)  and  another  for  the  vehicle  body  (Compose). 

CONFIGURATION  Compose(CONFIGURAT10N&  ql, CONFIGURATIONS;: 
q2,  CONFIGURATION&  q3,  double&  s,  double&  deltaTime) 
{    double  x,y, 

sinTheta  =  sin(ql.Theta), 

cosTheta  =  cos(ql.Theta); 

x  =  ql.Point.x  +  q2.Point.x*cosTheta  -  q2.Point.y*sinTheta; 

y  =  ql  .Point.y  +  q2.Point.x*sinTheta  +  q2.Point.y*cosTheta; 

q3.Point.x  =  x; 

q3. Point.y  =  y; 

q3.Theta  =  ql.Theta  +  q2.Theta; 

q3.Psi  =  ql.Psi  +  (omega  *  deltaTime);    /*  how  to  handle  move  left/right?  */ 
fprintf(f6,"%10.3f%10.3f%10.3f%10.3f%10.3f\nM, 

s,q3.Point.x,  q3.Point.y,q3.Theta,  q3.Psi); 
return  q3; 

}//  end  Compose 


CONFIGURATION  Compose2(CONFIGURATION&  ql, CONFIGURATIONS: 
q2,  CONFIGURATION&  q3)  /*posinon  */ 
{    double  x,y, 

sinTheta  =  sin(ql.Psi), 

cosTheta  =  cos(ql.Psi); 

x  =  ql.Point.x  +  q2.Point.x*cosTheta  -  q2.Point.y*sinTheta; 
y  =  ql. Point.y  +  q2.Point.x*sinTheta  +  q2.  Point.  y*cosTheta; 
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q3.Point.x  =  x; 
q3.Point.y  =  y; 
return  q3; 

}//  end  Compose2 

The  Compose  function  contains  several  lines  that  are  important  for  observations 
in  our  simulation.  The  calculation  of  q3.Theta,  as  mentioned  before  the  data  theta  should 
grow  in  positive  manner  as  the  vehicle  moves  from  its  initial  point  to  the  line  above  it. 
Once  the  line  that  is  being  tracked  has  been  reached  the  value  for  theta  goes  to  zero. 
Secondly  the  calculation  for  psi  shows  that  as  the  vehicle  moves  from  the  initial  position 
to  the  end  of  the  line  being  tracked,  the  value  of  psi  will  be  decremented  by 
omega*deltatime  (note  in  the  code  omega  is  defined  as  a  negative  number  [Appendix 
M]).  The  Compose2  function  is  important  because  it  provides  the  ability  to  compose  the 
body  orientation  (psi)  with  the  x  and  y  cooridinates  using  the  previously  defined  step. 

In  the  actual  simulation  the  values  of  theta  were  manipulated  to  ensure  the  vehicle 
would  track  the  next  line  above  (40  centimeters  higher)  on  the  next  step  through  the  loop 
(see  the  code  segment  below): 

q_xaxis. Pointy  =  q_xaxis. Pointy  +  40.0; 
if(ix%2==0){ 

q_xaxis.Theta     =  PI; 

q.Theta     =  PI; 

omega  =  fabs(omega); 
}else{ 

q_xaxis.Theta     =  0.0; 

q.Theta     =  0.0; 

omega  =  -omega; 

The  simulation  proved  successful  based  on  the  data  provided  in  Appendix  L.  Figure  6.4 
is  a  graphical  representation  of  the  simulation  data  in  Appendix  L.  Moreover,  all  the 
structures  are  in  place  in  the  SRK  [Appendix  J,  Consolidated  header  files]  to  implement 
the  sensing  motion.  If  more  time  was  available  for  this  thesis  the  sensing  motion  would 
have  been  implemented. 
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Figure  6.4:  Sensing  motion  simulation. 
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VII.  CONCLUSIONS 

A.         SUMMARY 

What  was  accomplished  as  a  result  of  this  research.  A  restatement  of  the  goals  or 
questions  addressed  of  this  is  necessary.  The  thesis  was  to  examine  the  following 
research  areas: 

•  What  kinematics  algorithms  must  be  developed  to  support  a  vehicle  with  three 
degrees  of  freedom  of  motion?  The  aforementioned  algorithms  must  support 
highly  flexible,  controlled,  and  precise  motion. 

•  What  types  of  controls  are  required  to  ensure  the  optimal  mix  of  driving  and 
steering  resources?  Moreover,  what  must  be  done  to  ensure  that  all  the 
resources  complement? 

•  How  can  the  knowledge  gained  in  the  aforementioned  research  areas  be  used 
to  develop  searching  motion? 

•  How  should  the  hardware  and  software  systems  be  implemented  to  support 
the  aforementioned  goals? 

The  kinematics  algorithms  developed  to  support  a  vehicle  with  3  degrees  of 
freedom  of  motion  can  be  divided  into  2  categories,  'low  level'  and  'high  level'.  The  first 
category  is  'low  level'.  The  'low  level'  algorithms  are  illustrated  in  Chapter  IV  and  in 
Appendix  E  (motor.c).  These  'low  level'  algorithms  are  concerned  with  taking  inputs  and 
passing  the  inputs  to  the  hardware.  The  empirical  results  from  the  direct  input  and 
servomotor  output  can  be  seen  in  Chapter  V.  The  'high  level'  category  considers  desired 
body  motion  and  its  transformation  into  wheel  motion  (see  Chapter  IV  and  Appendix  D). 
The  driver  function  in  movement. c  binds  the  high  and  low  level  categories  allowing  for 
highly  flexible,  precise,  and  controlled  motion  of  the  rotary  vehicle. 

The  controls  required  for  the  optimal  mix  of  driving  and  steering  resources  are 
developed  in  Chapter  V.  The  algorithms  and  their  implementation  as  described  in 
Chapter  V  removes  or  lessens  the  effect  of  variance  and  disparities  of  servomotor  output. 
This  ensures  an  optimal  mix  of  driving  and  steering  resources  and  that  resources  are 
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utilized  in  a  complimentary  manner.  Hence,  the  aforementioned  algorithms  gives  the 
desired  output  within  1%  error  for  performance  specifications. 

The  knowledge  gained  and  the  researched  discribed  in  Chapters  IV,  V,  and  VI  can 
be  used  to  implement  the  required  searching  motion  the  rotary  vehicle  needs  for  the  UXO 
effort.  Discoveries  and  knowledge  gained  during  development  of  the  Tornado, 
Sinusoidal,  and  Joystick  controlled  motions;  coupled  with  the  searching  motion 
simulation  can  be  used  to  implement  the  tangential  and  searching  motion  (see 
Appendices  C  and  D).  Moreover,  the  structures  are  presently  in  place  to  support  both 
tangential  and  searching  motions  in  Appendix  D. 

The  hardware  and  software  systems  are  implemented  as  described  in  Chapters  III 
and  IV.  This  hardware-software  implementation  is  tightly  coupled  and  ensures  proper 
communication  within  the  system.  This  communcation  allows  the  users  desired  inputs  to 
be  translated  into  vehicle  motion  in  real-time. 

B.         LESSONS  LEARNED 

The  Shepherd  rotary  vehicle  was  designed  by  Professor  Kanayama  but  built  by 
Mitsubishi  Heavy  Industries  in  Japan.  This  resulted  in  product  not  to  specification,  and  a 
lack  of  legible  documentation.  One  aspect  was  that  the  rotary  vehicle  could  not  fit  into 
the  lab  it  was  planned  for.  Because,  the  size  of  the  robot  was  not  correct.  Also,  the 
documentation  provided  was  in  Japanese,  big  problem.  None  of  the  authors  could  read  or 
speak  Japanese.  This  generated  numerous  faxes  and  telephone  calls  to  Mitsubishi  Heavy 
Industries  requesting  support  and  clarification.  Numerous  hours  were  lost  in  this 
endeavor.  Moreover,  some  of  the  wiring  diagrams  did  not  match  the  actual  wiring 
implementation  on  the  rotary  vehicle. 

The  compiler  used  is  GCC  version  2.7.2. 1 .  It  was  good  because  it  was  freeware. 
However,  for  cross-compiler  requirements  of  the  Motorola  68040  processor  it  left  a  lot  to 
be  desired.  Most  notably,  the  lack  of  all  basic  libraries.  All  input-output  functions  and 
math  functions  had  to  be  written  by  the  authors.  Again,  taking  an  inordinate  amount  of 
time  and  effort.    Here  the  authors  were  recreating  the  wheel,  per  se.    Also,  the  unusual 
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handling  of  structure  passing  required  the  development  of  several  work-arounds. 
Essentially,  standard  C-code  writing  had  to  be  modified  extensively.  The  Motorola 
68040  chip  was  designed  to  support  code  written  for  the  Motorola  68020.  This  may 
been  true  but  the  authors  determined  that  for  math  functions  this  was  not  the  case.  This 
also  may  be  related  to  the  compiler  (switches).  The  authors  contacted  Motorola  and 
Omnibyte  Corporations  for  help  in  dealing  with  these  matters  with  little  help  provided. 
In  the  majority  of  cases  the  help  provided  led  to  dead-ends.  Technicians  at  Omnibyte 
recommended  that  a  purchase  of  a  compiler  suitable  for  the  M68040  be  made. 

The  Taurus  Bug  (firmware  on  the  Taurus  Board)  provided  excellent  capabilities. 
However,  a  lot  of  time  was  required  to  use  the  tools.  On  many  occasions  numerous  hours 
were  spent  looking  through  loops  of  assembly  code.  This  would  not  have  been  the  case 
with  a  modern  debugger. 

Group  cross-pollenation  is  essential  for  a  project  with  multiple  disciplines 
involved.  For  example,  a  low  voltage  problem  on  the  rotary  vehicle  caused  the  CPU  to 
slow  or  halt  on  some  occasions.  Many  hours  were  spent  trying  to  determine  what  was 
wrong  with  the  C-coding  and  the  problem  was  hardware.  This  may  have  been  alleviated 
had  there  been  an  electrical  engineer  on  the  team.  Problems  like  this  were  exacerbated 
by  the  documentation  problem  mentioned  earlier.  Another  problem  was  the  overheating 
of  the  boards  on  the  VMEbus  (including  the  CPU)  resulting  in  system  shutdowns  as  well 
as  aberrations  in  CPU  behavior. 


C.         RECOMMENDATIONS  FOR  FUTURE  RESEARCH 

Recommendations  for  future  research  should  include: 


• 


• 


Implementation  of  tangential  motion,  the  rudiments  of  which  are  already  in 
place. 

Implementation  of  sensing  motion  which  is  required  for  the  percise,  accurate, 
and  safe  detection  of  UXO's. 


•     Transitioning  the  SRK  to  the  PC  environment  possibly  using  LINUX  freeware 
as  the  real-time  operating  kernal. 
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•  Implementation  of  a  M68040  specific  compiler  with  all  libraries. 

•  Addition  of  the  robot  arm  and  implementation  of  the  a  neural  net  and  expert 
system  for  UXO  identification.    Considerations  should  be  given  to  sensors 
such  as  a  magnetometer,  digital  camera  devices,  ground  penetrating  radar, 
xray  devices  and  GPS. 

•  Implementation  of  wireless  ethernet  or  hand  held  computing  devices  for 
control  and  monitoring  of  the  rotary  vehicle  by  a  user  who  is  not  co-located. 

•  Possible  missions  other  than  UXO.  For  instance,  an  unmanned  scout  vehicle 
or  mobile  sentry. 

It  is  obvious  from  the  above  recommendations  that  the  possible  uses  of  the  rotary  vehicle 

is  extensive  and  varied.  The  highly  flexible,  precise,  and  controlled  motion  of  the  vehicle 

makes  it  an  ideal  platform  for  many  ground  applications. 
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APPENDIX  A:  SOURCE  CODE  (MAKEFILE) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 

1  # * 

2  #  * 

3  #File:  MAKEFILE  * 

4  #  * 

5  #  Environment:  GCC  Compiler  v2.7.2  * 

6  #  Update:        02  February  1997  (Leonardy)  * 
7#               02  April  1997     (Ed  Mays)  * 

8  #  14  May  1997       (Leonardy,  added  utils030.*)  * 

9  #  Name:  Thorsten  Leonardy  * 

10  #  Purpose:       Makefile  for  project  S  H  E  P  H  E  R  D  .  * 

11  #  * 

12  #  Invoke:        make  comp  (to  generate  code)  * 

13  #  make  print  (to  print  all  files  to  printer  apl  in  Sp-51 1)  * 

14  #  make  clean  (to  clean  directory  from  object  files)  * 
15#                                                                                                   * 

1 6  # */ 

17 

18 

19  comp:  startup. o  shepherd. o  timer.o  serial. o  math.o  utils.o  utils030.o  user.o  \ 

20  motor.o  movement.o 

2 1  Id  -Ttext  Ox  1 0000  -Tdata  0x20000  -Tbss  0x30000  -Map  shepherd.map  -oformat 
srec  \ 
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22  -o  shepherd.TXT  startup.o  shepherd. o  timer.o  serial.o  math.o  utils.o  \ 

23  utils030.o  user.o  motor.o  movement.o 
24 

25  shepherd.o  :  shepherd.c 

26  gcc  -c  -m68040  -o  shepherd.o  shepherd.c 
27 

28  timer.o  :  timer.c 

29  gcc  -c  -m68040  -o  timer.o  timer.c 
30 

31  serial.o  :  serial. c 

32  gcc  -c  -m68040  -o  serial.o  serial. c 
33 

34  #servo.o  :  servo. c 

35  #  gcc  -c  -m68040  -o  servo.o  servo. c 
36 

37  utils.o  :  utils.c 

38  gcc  -c  -m68040  -o  utils.o  utils.c 
39 

40  utils030.o  :  utils030.c 

4 1  gcc  -c  -m68040  -o  utils030.o  utils030.c 
42 

43  user.o  :  user.c 

44  gcc  -c  -m68040  -o  user.o  user.c 
45 

46  motor.o  :  motor.c 

47  gcc  -c  -m68040  -o  motor.o  motor.c 
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48 


49  movement.o  :  movement.c 

50  gcc  -c  -m68040  -o  movement.o  movement.c 
51 

52  startup. o  :  startup. s 

53  as  -o  startup.o  startup. s 
54 

55  math.o:  math.c 

56  gcc  -c  -m68040  -o  math.o  math.c 
57 

58 

59  #  This  cleans  out  everything  except  the  Makefile, 

60  #  and  source  files 

61  clean:;  rm  -f  *.o  core 
62 

63  #  This  prints  all  source  files  to  the  printer  apl  in  Sp-51 1 

64  print:;  enscript  -2r  -g  -Papl  makefile  shepherd. map  shepherd. h  shepherd.c  \ 

65  user.h  user.c  utils.h  utils.c  utils030.c  serial.h  serial.c  servo.h  servo.c  \ 

66  timer.h  timer.c  movement. h  movement.c  wheeldrive.h  wheeldrive.c  \ 

67  math.h  math.c  startup. s 
68 

69  #  This  prints  all  source  files  to  the  printer  spl  in  Sp-527 

70  prspl:;  enscript  -2r  -g  -Pspl  makefile  shepherd. map  shepherd. h  shepherd.c  \ 

71  user.h  user.c  utils.h  utils.c  utils030.c  serial.h  serial.c  servo.h  servo.c  \ 

72  timer.h  timer.c  movement. h  movement.c  navigat.h  navigat.c  wheeldrive.h 

75 


wheeldrive.c  \ 
73  math.h  math.c  motor.h  motor.c  startup. s 

74 

ID  if 

76  #  End  of  makefile 

I   I     TT  \ 
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APPENDIX  B:  SOURCE  CODE  (SHEPHERD.C) 

The  following  code  was  modified  by:  Professor  Kanayama*  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 


1  /* 

2  * 

3  *  File:  SHEPHERD  .C 

4  * 

5  *  Environment:  GCC  Compiler  v2.7.2 

6  *  Last  update:  29  January  1997 

7  *  Name:  Thorsten  Leonardy 

8  *  Purpose:       Provides  the  kernel  for  SHEPHERD. 

9  * 

10  *  Compiled:      >gcc  -c  -m68040  -o  shepherd. o  shepherd.c 

11  * 

1 2  * 

13 

14  #include  "shepherd. h"  /*  general  defines  */ 

15  #include  "movement.h" 
16 

17 

18/* * 

19  *  constant  character  strings  * 

20  * */ 

21  unsigned  char  JOYSTICK[26]=  { 
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/*  'x=000 ' 

*/ 

/*  'y=000 ' 

*/ 

/*  *z=000' 

*/ 

22  27,91,49,56,59,54,48,72,  /*  ESC  [  T  '8' ;  '6'  '0'  H  */ 

23  120,61,48,48,48,32, 

24  121,61,48,48,48,32, 

25  122,61,48,48,48,0}; 
26 

27  unsigned  int  wheelEncoder[8]; 

28  unsigned  char  bcdString[25]= 

29  {24,27,91,48,50,59,52,48,72, 

30  48,48,48,48,48,48,48,48,48,48,48,48,48,48,48,48 } ; 
31 

32  /*  clock  contains  the  current  date/time  updated  every  second  in  ascii  format  */ 

33  /*  it  starts  with  a  count  byte,  the  cursor  positioning  sequence  followed  by    */ 

34  /*  the  date-group  and  the  time  group  in  the  form:  */ 

35  /*    clock=" mm-dd-yy  hh:mm:ss."  where  '.'  is  part  of  count  or  ESC     */ 

36  /*  sequence.  */ 
37 

38  /*unsigned  char  clock[27]={   */ 

39/*  25,27,91,48,49,59,54,52,72,    */      /*  ESC  [  '0' T  ;  '6'  '4'  H  */ 

40  /*         109,109,45,100,100,45,121,121,32,  */  /*  'mm-dd-yy  '  */ 

41  /*  104,104,58,109,109,58,115,115,0};*/  /*  'hhmumss'  */ 
42 

43  void  advanceCount() 

44  { 

45  edCounter-H-; 

46  } 
47 
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48  main() 

49  { 

50  initBoards();       /*  initialize  boards  */ 

51  initMovement();     /*  initialize  movement  */ 

52  sioInit();  /*  initialize  68C681  DU ART  for  serial  I/O              */ 

53  timerStartO;       /*  initialize  and  start  timer-5  fopr  motion  control     */ 

54  /*  setup68030() ;  */  /*  setup  OMNI  Module  0  for  serial  I/O  to  VT 1 00  */ 

55  enable();  /*  enable  all  interrupts,  user  mode                    */ 

56  user();  /*  let  user  handle  the  main  portion                     */ 

57  disable();  /*  disable  interrupts,  supervisor  mode                 */ 
58 

59  /*  here  goes  downloading  stuff  for  analysis  ...  (i.e.  copy  memory  */ 

60  /*  from  Taurus  Main  memory  to  host  computer  (Laptop  or  SparcStation)       */ 
61 

62  return; 

63 

64  }    /*  end  of  main()  */ 

65 

66 

67 

68 /******************************************************************** 

69    End  of  shepherd.c 

71 

72 
73 

79 
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APPENDIX  C:  SOURCE  CODE  (USER.C) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 


1  /* * 

2  *  * 

3  *  File:  USER.C  * 

4  *  * 

5  *  Environment:  GCC  Compiler  v2.7.2  * 

6  *  Last  update:   18  February  1997  * 

7  *  Name:  Thorsten  Leonardy  * 

8  *  Purpose:       Provides  the  userpart  for  SHEPHERD.  * 

10  *  Compiled:      >gcc  -c  -m68040  -o  user.o  user.c  * 

j  j    *  * 

12  * */ 

13 

14  #include  "shepherd.h"  /*  general  defines  */ 

15 

16 

17  /* * 

18  *  Global  variables  for  test  program  * 

19  * - */ 

20 

21  extern  unsigned  char  inPortA;  /*  defined  in  serial.c  */ 

22  extern  unsigned  char  vtl00xy[9];  /*  ESC-Sequence  for  Cursor  Position  */ 
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23  extern  unsigned  char  clrSCR[5];    /*  ESC-Sequence  for  clear  Screen     */ 

24  extern  unsigned  char  clrLine[6];  /*  ESC-Sequence  for  clear  line       */ 

25  extern  unsigned  char  prtSCR[4];    /*  ESC-Sequence  for  print  screen     */ 

26  extern  unsigned  char  cursorOFF[5];/*  ESC-Sequence  for  cursor  off      */ 
27 

28  extern  void  gotoXY(int,int); 

29  extern  int  wheelEncoder[8];    /*  defined  in  shepherd.c  */ 
30 

31 

32  extern  char  bcdString[];  /*  defined  in  shepherd.c  */ 

33 

34  unsigned  short  velocity=0; 

35 

36 

37  /* * 

38  *  constant  character  strings  * 

39  * */ 

40 

41  #define  MENU_LINES  24 

42  char  *menu[MENU_LINES]={ 

43  "SHEPHERD  Main  Menu  (Last  Update:  27  Feb  97)\n\r", 

44  "  1  —5 — 0— -5— -0— -5 — 0—5 — 0— -5— -0 — 5 — 0— -5 — 0 — 5 — 0\n\r", 

45  "\n\r" 

46  "Please  choose:  Diagnostics\n\r", 

47  "  \n\r", 

48  /*  character  pressed,  analize  ...  */ 
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49 
50 
51 

52 
53 
54 
55 
56 
57 
58 
59 
60 


l)Stop  \n\r", 

2)  Straight  Motion  (Autonomous)     \n\r", 

3)  Straight  Motion  by  Joystick      \n\r", 

4)  XY-Motion  by  Joystick  \n\r", 

5)  Rotate  \n\r", 

6)  Sinusoidal  Motion  \n\r", 

7)  Tornado  (External  Center  of  Rotation)\n\r", 

8)  Tornado  (Internal  Center  of  Rotation)\n\r", 

9)  Tangential  Motion  \n\r", 
0)  Exit  Program                    \n\r", 

a)  Tangential  Motion(II)  \n\r", 

t)  Test  wheel  Angle  4  \n\r", 


61  "  \n\r", 

62  "  \n\r", 

63  "  \n\r", 

64  "  \n\r", 

65  "  \n\r", 

66  "  \n\r", 

67  " \n\r", 

68  "  " 
69}; 
70 

71 
72 
73 
74 
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75/* 

76  *  displayMenu() 

77  * 

78  *  Environment:  GCC  Compiler  v2.7.2 

79  *  Last  update:  27  January  1997 

80  *  Name:  Thorsten  Leonardy 

8 1  *  Purpose:       This  function  outputs  a  menu  to  the  screen 

82  * 

83  * 

84  void  displayMenu(void) 

85  { 

86  inti; 

87  sioOut(0,clrSCR);    /*  clear  screen  */ 

88  gotoXY(U); 

89  for  (i=0;i<MENU_LINES;  i++) 

90  sioOut(0,menu[i]); 

91  return; 

92  } 
93 
94 
95 

96/* 

97  *  status() 

98  * 

99  *  Environment:  GCC  Compiler  v2.7.2 

100  *  Last  update:    18  February  1997 
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101  *  Name:  Thorsten  Leonardy  * 

102  *  Purpose:       This  function  outputs  a  status  line  at  the  bottom  of  the     * 

103  *  screen.  * 

104  * */ 

105  void  status(unsigned  char  *p) 
106{ 

107  gotoXY(24,l);         /*  position  cursor  */ 

108  sioOut(0,clrLine);    /*  clear  line       */ 

109  sioOut(0,p);  /*  print  text       */ 

110  return; 
111} 

112 
113 

1 14/* * 

115  *  convertBCD()  * 

116*  * 

1 17  *  Environment:  GCC  Compiler  v2.7.2  * 

1 18  *  Last  update:  20  February  1997  * 

1 19  *  Name:  Thorsten  Leonardy  * 

120  *  Purpose:       This  function  converts  an  unsigned  integer  to  a  BCD  string  * 

121  *  of  16  characters.  The  value  is  right  justified  with  leading  * 

122  *  zeros.  * 

123  * */ 

124 void  convertBCD(unsigned  char  *s,  unsigned  int  data) 

125{ 

126    inti=15; 
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127 

128  for  (i=15;  i>=0;  i— )  {  /*  write  16  Bytes,  adjust  integer  to  right  of  string  */ 

129  *(s+i)=48+(data%10); 

130  data=data/10; 

131  } 

132  return; 
133} 

134 

1 35/* * 

136  *  converting)  * 

137*  * 

138  *  Environment:  GCC  Compiler  v2.7.2  * 

139  *  Last  update:   1 1  June  1997  * 

140  *  Name:  Thorsten  Leonardy,  Yutaka  Kanayama  * 

141  *  Purpose:       This  function  converts  a  signed  integer  to  a  BCD  string      * 

142  *  of  16  characters.  The  value  is  right  justified  with  leading  * 

143  *  zeros.  * 

144  * */ 

145 void  convertInt(unsigned  char  *s,  int  data) 

146{ 

147    int  i=15; 

148 

149  if(data>=0) 

150  *s  =  32;  /*  space       for  positive  number  */ 

151  else 

152  { 
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153  *s  =  45;  /*  minus  sign  for  negative  number  */ 

154  data  = -data; 

155  } 
156 

157  for  (i=15;  i>=l;  i-)  {  /*  write  16  Bytes,  adjust  integer  to  right  of  string  */ 

158  *(s+i)=48+(data%10); 

159  data=data/10; 

160  } 

161  return; 
162} 

163 
164 
165 
1 66/* * 

167  *  wheelDrive()  * 

168  *  * 

169  *  Environment:  GCC  Compiler  v2.7.2  * 

170  *  Last  update:  27  February  1997  * 
171*  Name:          Thorsten  Leonardy  * 

172  *  Purpose:       This  function  drives  the  specified  wheel  with  required       * 

173  *  velocity.  * 

174  *  * 

175  *    - */ 

176void  wheelDrivel  (unsigned  short  wheel,  unsigned  short  velo) 
177{ 

178    unsigned  short  *servoOut=(unsigned  short  *)0xffff0482;  /*  analog  out  */ 
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179    unsigned  int    *servoControl=(unsigned  int*)OxffffffOO;     /*  Data  out    */ 

180 

181     *servoOut=velo;        /*  set  velocity  first  */ 

182 

183  if  (wheel) 

184  *servoControl=0x00000004; 

185  else 

186  *servoControl=0x00000000; 
187 

188  return; 

189} 

190 

191 

192 

193 

194 

1 95/* * 

196  *  updateWheelStatusO  * 

197  *  * 

198  *  Environment:  GCC  Compiler  v2. 7. 2  * 

199  *  Last  update:  27  February  1997  * 

200  *  Name:  Thorsten  Leonardy  * 

201  *  Purpose:       This  function  reads  the  current  shaft  encoder  readings  for  * 

202  *  all  eight  servo  motors  and  outputs  them  to  the  screen.       * 

203  *  * 

204  *  unsigned  int  wheelEncoder[8]  -  array  to  hold  the  shaft  encoder  readings  * 
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205  *  unsigned  char  *bcdString       -  string  to  hold  converted  encoder  reading  * 
206*  * 

207  * */ 

208void  updateWheelStatus(void) 

209  { 

210 

21 1    unsigned  short  i,posx,posy; 

212 

213    readWheelStatus(wheelEncoder);  /*  read  wheel  status:  File  servo.c  */ 

214 

215  posx=8;  /*  x-position  on  screen  for  reading  motor  1  */ 

2 1 6  posy=40;  /*  y-position  on  screen  for  reading  motor  1  */ 
217 


218  for  (i=0;  i<8;  i++)  { 

219  posx=8+i%4; 

220  posy=40+20*(i/4); 

221  bcdString[3]=48+posx/10; 

222  bcdString[4]=48+posx%10; 

223  bcdString[6]=48+posy/10; 

224  bcdString[7]=48+posy%10; 


/*  position  for  x  */ 

/*  position  for  y  */ 

/*  convert  tens  to  ascii  */ 

/*  convert  ones  to  ascii  */ 
/*  convert  tens  to  ascii  */ 
/*  convert  ones  to  ascii  */ 


225  convertBCD(bcdString+9,wheelEncoder[i]);      /*  convert  reading  itself  */ 

226  WRITE_ENCODER();  /*  output  ascii  */ 

227  }   /*  end  of  for*/ 
228 

229    return; 
230} 
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231 
232 
233void  displayDriveSpeed() 

234  { 

235  double  Speed00,speed0,speedl,speed2,speed3; 
236 

237  disable(); 

238  speedOO=desiredSpeeds[0]; 

239  speedO=Display_Speeds[0]; 

240  speed l=Display_Speeds[l]; 

241  speed2=Display_Speeds[2]; 

242  speed3=Display_Speeds[31; 

243  Display_Speeds[0]=0.0; 

244  Display_Speeds[l]=0.0; 

245  Display_Speeds[2]=0.0; 

246  Display_Speeds[3]=0.0; 
247 

248  enable(); 

249 

250       convertInt(bcdString+9,speedOO); 


251 

bcdString[3]='0'; 

252 

bcdString[4]=3'; 

253 

bcdString[6]='4'; 

254 

bcdString[7]='0'; 

255 

sioOut(0,bcdString); 

256       convertInt(bcdString+9,speed0); 
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257 

bcdString[3]='0'; 

258 

bcdString[4]='3'; 

259 

bcdString[6]='6'; 

260 

bcdString[7]='0'; 

261 

sioOut(0,bcdString); 

262 

convertInt(bcdString+9,speedl ); 

263 

bcdString[3]='0'; 

264 

bcdString[4]='4'; 

265 

bcdString[6]='6'; 

266 

bcdString[7]='0'; 

267 

sioOut(0,bcdString); 

268 

convert  Int(bcdString+9,speed2); 

269 

bcdString[3]='0'; 

270 

bcdString[4]='5'; 

271 

bcdString[6]=6'; 

272 

bcdString[7]='0'; 

273 

sioOut(0,bcdString); 

274 

convertInt(bcdString+9,speed3); 

275 

bcdString[3]='0'; 

276 

bcdString[4]='6'; 

277 

bcdString[6]='6'; 

278 

bcdString[7]='0"; 

279 

sioOut(0,bcdString); 

280  i 

eturn; 

281} 

282 
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283  void  display DriveSteer() 

284  { 

285  double  steerOO,steerO,steerl,steer2,steer3; 
286 

287  disable(); 

288  steerOO=Steer_Digits[0]; 

289  /*  steerOO=desiredAngleRates[0];  */ 

290  steerO=desiredAngleRates[0]*  1000; 

291  steerO=Display_Steers[0]; 

292  steer l=Display_Steers[l]; 

293  steer2=Display_Steers[2]; 

294  steer3=Display_Steers[3]; 

295  Display_Steers[0]=0.0; 

296  Display_Steers[l]=0.0; 

297  Display_Steers[2]=0.0; 

298  Display_Steers[3]=0.0; 
299 

300  enable(); 
301 

302  convertInt(bcdString+9,steer00); 

303  bcdString[3]='0'; 

304  bcdString[4]='3'; 

305  bcdString[6]=,4'; 

306  bcdString[7]=,0'; 

307  sioOut(0,bcdString); 

308  convertInt(bcdString+9,steerO); 
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309 

bcdString[3]=,0'; 

310 

bcdString[4]='3'; 

311 

bcdString[6]='6'; 

312 

bcdString[7]='0'; 

313 

sioOut(0,bcdString); 

314 

convertInt(bcdString+9,steerl); 

315 

bcdString[3]='0'; 

316 

bcdString[4]='4'; 

317 

bcdString[6]='6'; 

318 

bcdString[7]='0'; 

319 

sioOut(0,bcdString); 

320 

convertInt(bcdString+9,steer2); 

321 

bcdString[3]='0'; 

322 

bcdString[4]='5'; 

323 

bcdString[6]='6'; 

324 

bcdString[7]=,0'; 

325 

sioOut(0,bcdString); 

326 

convertInt(bcdString+9,steer3); 

327 

bcdString[3]='0'; 

328 

bcdString[4]='6'; 

329 

bcdString[6]='6'; 

330 

bcdString[7]='0'; 

331 

sioOut(0,bcdString); 

332  return; 

333} 

334 
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3 35  void  display Angles() 

336{ 

337  double  steerO,steerl,steer2,steer3; 

338 

339  disable(); 

340 

34 1  steerO=actualAngles[0]*  1000*RadsToDegrees; 

342  steerl=actualAngles[l]*1000*RadsToDegrees; 

343  steer2=actual Angles [2]*  1000*RadsToDegrees; 

344  steer3=actualAngles[3]*  1000*RadsToDegrees; 
345 

346  enable(); 
347 

348  convertInt(bcdString+9,steerO); 

349  bcdString[3]='0'; 

350  bcdString[4]='3'; 

351  bcdString[6]='6'; 

352  bcdString[7]='0'; 

353  sioOut(0,bcdString); 

354  con  vertInt(bcdString+9, steer  1 ); 

355  bcdString[3]='0'; 

356  bcdString[4]='4'; 

357  bcdStnng[6]='6'; 

358  bcdString[7]='0'; 

359  sioOut(0,bcdString); 

360  convertInt(bcdString+9,steer2); 
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361  bcdString[3]='0'; 

362  bcdString[4]='5'; 

363  bcdString[6]='6'; 

364  bcdString[7]='0'; 

365  sioOut(0,bcdString); 

366  convertInt(bcdString+9,steer3); 

367  bcdString[3]=*0'; 

368  bcdString[4]='6'; 

369  bcdString[6]='6'; 

370  bcdString[7]='0'; 

371  sioOut(0,bcdString); 

372  return; 
373} 

374 void  display VehicleConfigO 

375  { 

376  double  coordx,  coordy,  heading,  kappa; 
377 

378  disable(); 
379 

380  coordx  =  vehicle.coord.x; 

381  coordy  =  vehicle.coord.y; 

382  heading  =  vehicle. heading; 

383  kappa    =  vehicle. kappa; 
384 

385  enable(); 
386 
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387 

convertInt(bcdString+9,coordx); 

388 

bcdString[3]='0'; 

389 

bcdString[4]='3*; 

390 

bcdString[6]='6'; 

391 

bcdString[7]='0'; 

392 

sioOut(0,bcdString); 

393 

convertInt(bcdString+9,coordy); 

394 

bcdString[3]='0'; 

395 

bcdString[4]='4'; 

396 

bcdString[6]='6'; 

397 

bcdString[7]='0'; 

398 

sioOut(0,bcdString); 

399 

convertInt(bcdString+9,heading); 

400 

bcdString[3]='0'; 

401 

bcdString[4]=5'; 

402 

bcdString[6]='6'; 

403 

bcdString[7]='0'; 

404 

sioOut(0,bcdString); 

405 

convertInt(bcdString+9,kappa); 

406 

bcdString[3]='0'; 

407 

bcdString[4]='6'; 

408 

bcdString[6]='6'; 

409 

bcdString[7]=0'; 

410 

sioOut(0,bcdString); 

411  i 

eturn; 

412} 
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413/* * 

414*user()  * 

415  *  * 

416  *  Environment:  GCC  Compiler  v2.7.2  * 

417  *  Last  update:   18  February  1997  * 
418*  Name:          Thorsten  Leonardy                                           * 
419  *  Purpose:       This  function  provides  the  user  shell.  * 
420*                                                                          * 

421  * */ 

422 

423  void  user(void) 

424  { 

425  inta; 

426  char  *s; 

427  unsigned  int    *servoControl=(unsigned  int  *)VME2170;/*  test  only  */ 

428  displayMenu();    /*      display  menu        */ 

429  do 

430  { 

431  inPortA='?';  /*  reset  character  */ 

432  while(inPortA=='?');       /*  wait  for  character  to  be  typed  in  */ 

433  /*  character  pressed,  analize  ...  */ 

434  switch(inPortA) 

435  { 

436  caseT  :  if  (mode  !=  5)       /*  Stop  */ 

437  { 

438  modeOstate  =  0; 
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439  mode  =  1 ; 

440  while  (modeOstate  ==0)  { } ; 

441  disable(); 

442  align(); 

443  enable(); 
444 

445 
446 
447 
448 

449  /*  *servoControl=0x00429429;  test  by  Ed  */ 

450  modeOstate  =  2; 

451  } 

452  else 

453  { 

454  mode  =  1 ; 

455  disable(); 

456  alignAfterRotate(); 

457  enable(); 

458  /*  *servoControl=0x00429429;    test  by  Ed*/ 
459 

460  } 

461  initMovementQ; 

462  break; 
463 

464         case  '2'  :  mode  =  2;  /*  Straight  Motion  (Autonomous)  */ 
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465  break; 

466 

467  case  '3' :  mode  =  3;  /*  Straight  Motion  by  Joystick  */ 

468  break; 
469 

470  case  '4' :  mode  =  4;  /*  X-Y  Motion  by  Joystick  */ 

471  /*  for(a=0;a<100;a++){  */ 

472  /*      while  ((edCounter  %  200  !=  0)  &&  (a  !=  100))  {*/ 

473  /*         displayAngles();  */ 

474  /*      }  */ 

475  /*   }  */ 

476  break; 
477 

478  case  *5'  :  mode5state  =  0;    /*  Rotate       */ 

479  mode  =  5; 

480  break; 
481 

482  case  '6'  :  mode  =  6;  /*  Sinusoidal  Motion  */ 

483  break; 
484 

485  case  T  :  mode  =  7;  /*  Tornado  (Center  of  Rot  External)     */ 

486  break; 
487 

488  case  '8'  :  mode  =  8;  /*  Tornado  (Center  of  Rot  Internal)     */ 

489  break; 


490 
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491  case  '9' :  mode  =  9;  /*  Tangential  Motion  */ 

492  initTangent(); 

493  while(l){ 

494  while(edCounter%200  !=  0) { } ; 

495  displayVehicleConfigO; 

496  }; 

497  break; 
498 

499  case  'a'  :  mode  =  10;  /*  Tangential  Motion  (II)  */ 

500  break; 
501 

502  case  't' :  modeTstate  =  0;  /*  Steering  test  mode  */ 

503  /*  Flag=l;  initialized  in  movement.c  */ 

504  mode  =  100; 

505  while  (1) 

506  { 

507  oldFlag  =  Flag; 

508  while  (Flag  =  oldFlag)  { } 

509  displayAnglesQ; 

510  } 

511  break; 
512 

513  default  :  break; 

514  }  /*  end  of  switch  */ 

515  }  while(inPortA!='0');/*  end  of  while,  exit  with  '0'  entered  at  keyboard  */ 


516 
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517  sioOut(0,clrSCR);       /*  clear  screen  */ 

518  sioOut(0,"\n\r\nV);  /*  some  cr,lf    */ 
519 

520  return; 
521 

522  while(l) 

523  { 

524  while(edCounter%200  !=  0) { } ; 

525  /*  displayJoyStick();    */ 

526  displayDriveSteer(); 

527  /*  display Angles();     */ 

528  }; 

529  sioOut(0,cursorOFF);  /*  switch  cursor  off  (no  blink)  */ 
530 

531}  /*  end  of  user()  */ 

532 

533 

534 

535asm(" 

536  .even 

537  .text 

538  .globl  _WRITE_ENCODER 
539 
540_WRITE_ENCODER: 

541 

542  pea.l  _bcdString 
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543 

544  trap  #15 

545  dew  0x0023 
546 

547       rts 

548"); 

549 

550 

551 

552/**************************************************************** 

553  Endofuser.c 

555 
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APPENDIX  D:  SOURCE  CODE  (MOVEMENT.C) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 

1  #include  "shepherd.h" 

2  #include  "movement.h" 

3  #include  "math.h" 
4 

5  /* * 

6  *  Main  * 


8  void  driver() 

9  { 

10  readEncoders();    /*  Read  Drive/Steer  Motors    */ 

1 1  computeActualRates(); 

12  /*accumulateDriveSpeed();    only  for  wheel  speed  displaying  */ 

13  accumulateDriveSteer(); 

14  bodyMotion(); 

15  wheelMotionQ; 

16  /*    testDrivel();  */ 

17  driveMotorsQ; 

18  advanceCount(); 
19} 
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20 
21 

22  /* * 

23  *  Initialize  Movement:  * 

24  *  intialize  Configuration  and  vehicle  motion  * 

25  * */ 

26  void  initMovementQ 


27  { 

28 

int  ix; 

29 

30 

Flag=l; 

31 

oldMode  = 

=  0; 

32 

mode  =  1 ; 

33  Omega_Speed=0.0; 

34  testCounter=0; 

35  edCounter=0; 

36  pathLength=0.0; 
37 

38  Kl[0]=11.448;/*0<=speed<=5,*/ 

39  Kl[  11=1 1.500; 

40  Kl  [2]=  11.496; 

41  Kl[31=12.375; 
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42 

43  K2[0]=11.500;/*5>speed<8*/ 

44  K2[l]=  11.500; 

45  K2[2]=  11.644; 

46  K2[3]=  12.000; 
47 

48 

49  K3[0]=11.611;/*8>=speed<20*/ 

50  K3[l]=l  1.585; 

51  K3[2]=l  1.686; 

52  K3[3]=l  1.840; 
53 

54  K4[0]=  1 1 .7 1 1  ;/*20>=speed<=70*/ 

55  K4[  1]=11.659; 

56  K4[2]=l  1.705; 

57  K4[3]=l  1.727; 
58 

59  K6[0]=11.710;/*70>speed<K5*/ 

60  K6[  1]=11.700; 

61  K6[2]=  11.700; 

62  K6[3]=l  1.715; 
63 
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64 

65  DigitToCmDrive[0]=  +0.001 1369287;/*  driving  constant  cm/count 
digitToRadDrive*  1 8.9cm*/ 

66  DigitToCmDrive[l]=  -0.001 1369287; 

67  DigitToCmDrive[2]=  +0.001 1369287; 

68  DigitToCmDrive[3]=  -0.001 1369287; 
69 

70 

71  motion. Speed=0.0; 

72  motion. Theta=0.0; 

73  motion. Omega=0.0; 
74 

75  radius  =  100; 
76 

77  vehicle.coord.x=0.0; 

78  vehicle.coord.y=0.0; 

79  vehicle. heading=0.0; 

80  vehicle.  kappa=l /radius; 
81 

82  ai[0]  =  40.0;  ai[l]  =  40.0;  ai[2]  =-40.0;  ai[3]  =-40.0; 

83  bi[0]  =-40.0;  bi[  1  ]  =  40.0;  bi[2]  =-40.0;  bi[3]  =  40.0; 
84 
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85 

86  joyStick.x  =  0.0; 

87  joyStick.y  =  0.0; 
88 

89  setupPolar(whp); 
90 

9 1  for  (ix  =0;  ix  <ARRAY_SIZE;  ix++)  { 

92  PreviousCountSpeed[ix]=99999999; 

93  PreviousCountSteer[ix]=99999999; 

94  Display_Speeds[ix]=0.0; 

95  Display_Steers[ix]=0.0; 

96  actualAngles[ix]=0.0; 

97  desiredSpeeds[ix]  =  0.0; 

98  desiredAngleRates[ix]  =  0.0; 

99  desiredAngleRatesO[ix]  =  0.0; 

100  desiredAngles[ix]=0.0; 

101  desiredAnglesO[ix]=0.0; 

102  WheelDirActO[ix]=  1.0e8; 

103  WheelDirAct[ix]  =  0.0; 

104  WheelDirDes[ix]  =  0.0; 

105  steerReadings[ix]=0.0;  /*  not  used  only  testing  */ 

106  driveReadings[ix]=0; 

107 


107  } 

108} 

109 

110 

HI/* * 

112  *SetupPolar  * 

113* */ 

1 14void  setupPolar(polar  whp[4]) 

1 15  { 

1 16  whp[0].rho  =  whp[l].rho  =  whp[2].rho  =  whp[3].rho  =  56.5685425; 

117  /*  distances  =  40  *  sqrt(2)  */ 

118  whp[0].alpha  =  -QPI;  /*  front  right  wheel  1    */ 

119  whp[l]. alpha  =  QPI;  /*  front  left  wheel  2    */ 

120  whp[2].alpha  =  -3.0*QPI;  /*  rear  right  wheel  3    */ 

121  whp[3].alpha=  3.0*QPI;  /*  rear  left  wheel  4    */ 
122} 

123 
124 
1 25/* * 

126  *  bodyMotion  —  Updates  Vehicle  * 

127  * */ 

128void  bodyMotion() 
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129{ 

1 30  double  vO,  omegaO, 

131  linSpeed=  4.0, 

132  linAcc  =  1.0, 

133  rotSpeed=0.1,  /*  0.05,         */ 

134  rotAcc  =0.025,  /*  0.0125;       */ 

135  RPI      =QPI*1.5;       /*  67.5  degrees  */ 

1 36  double  theta,  omega,  speed; 
137 

138  speed  =  motionO.Speed  =  motion. Speed;    /*  save  the  previous  motion    */ 

139  theta  =  motionO.Theta  =  motion.Theta;    /*  for  computing  derivatives  */ 

140  omega  =  motionO. Omega  =  motion. Omega; 
141 

142  switch(mode){ 

143  case  1: 

144  if  (modeOstate  ==  2) 

145  break; 

1 46  if  ( (Speed_Digits[0]  ==  0)  &&  (Speed_Digits[  1  ]  ==  0)  && 

1 47  (Speed_Digits[2]  ==  0)  &&  (Speed_Digits[3]  ==  0)  && 

1 48  (Steer_Digits[0]  ==  0)  &&  (Steer_Digits[  1  ]  ==  0)  && 

149  (Steer_Digits[2]  ==  0)  &&  (Steer_Digits[3]  ==  0)) 

1 50  modeOstate  =  1 ; 
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151  /*  allStopO;     will  be  inserted  later  */ 

152  break; 
153 

154  case  2: 

155  speed  =  min(speed  +  2.0*DeltaT,  10.0); 

156  break; 
157 

158  case  3:  /*  Straight  Motion  by  Joystick  */ 

159  readJoyStick();  /*  ejm  19  July  97                */ 

1 60  speed  =  -joyStick.y*0. 1 ; 

161  theta  =  0.0; 

162  omega  =  0.0; 

163  break; 
164 

165  case  4:     /*  X-Y  Motion  by  Joystick  */ 

166  readJoyStick();  /*  ejm  19  July  97*/ 

167  speed  = -joyStick. y*0.1;      /*  speed  control  */ 

168  theta  =  -joyStick. x*0.02;    /*  steering  control  */ 

169  if  (theta  >  HPI)  theta  =  HPI; 

170  if  (theta  < -HPI)  theta  =  -HPI; 

171  /*  omega  = -joyStick. omega*0.1;*/  /*    24  July  97*/ 

172  break; 
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173 

174  case  5: 

1 75  if  (mode5state  ==  1 )  { 

176  readJoyStick(); 

1 77  speed  =  -joyStick.y*0. 1 ; 

178  } 

179  break; 
180 

1 8 1  case  6:    /*  sinusoidal  motion  */ 

182  speed  =  min(speed  +  HnAcc*DeltaT,  10.0); 

183  speed  = speed; 

184  if  (speed  ==  10.0)  { 

1 85  pathLength  +=  DeltaT*speed; 

186  theta  =  0.4  *  sin(pathLength/20.0);  /*  sine  curve  motion  */ 

187  } 

188  break; 
189 

190  case  7:  /*  Tornado  External    */ 

1 9 1  speed  =  min(speed  +  1 .0*DeltaT,  8.0); 

192  if  (  speed  ==  8.0) 

1 93  omega  =  min(omega  +  0.0 1 25*DeltaT,  0. 1 );     /*  radius  =  80  cm  */ 

194  break; 
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195 

196  case  8:  /*  Tornado  Internal    */ 

197  speed  =  min(speed  +  1 .0*DeltaT,  8.0); 

198  if  (speed  ==8.0) 

199  omega  =  min(omega  +  0.025 *DeltaT,  0.2);      /*  radius  =  40  cm  */ 

200  break; 
201 

202  case  9:  /*  tangential  motion  */ 

203  tangentialMotion(); 

204  break; 
205 

206  case  10:  /*  tangential  motion  (II)  */ 

207  speed  =  min(speed  +  linAcc*DeltaT,  8.0); 

208  break; 
209 


210 

case  100 

211 

break; 

212 

} 

213 

214  if(mode!=9){ 

215  motion. Speed  =  speed; 

2 1 6  motion.Theta  =  theta; 
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217       motion. Omega  =  omega; 
218 

219  vehicle. heading  =  vehicle.heading  +  motion. Omega*DeltaT; 

220  vehicle.coord.x  =  vehicle.coord.x  +  motion. Speed*DeltaT  *  cos(motion.Theta); 

221  vehicle.coord.y  =  vehicle.coord.y  +  motion. Speed*DeltaT  *  sin(motion.Theta); 
222 

223  speedDot=(motion.Speed  -  motionO.Speed)/DeltaT; 

224  thetaDot=(motion.Theta  -  motionO.Theta)/DeltaT; 

225  omegaDot=(motion. Omega  -  motion0.Omega)/DeltaT; 

226  } 
227} 
228 
229 

230/* * 

231  *  wheelMotion  * 

232  * */ 

233void  wheelMotion() 

234  {       /*the  function  that  truly  belongs  here  is  in  calculate.org  */ 

235  int  i; 

236  double  v  1  x,  v  1  y,  v  1  yv  1  xRatio; 

237  double  theta=motion.Theta, 

238  omega=motion.  Omega, 

113 


239  speed=motion.  Speed, 

240  Omega2=omega*omega, 

241  Omega3=Omega2*omega, 

242  beta,ro,ro2, 

243  wheelAngleV; 
244 

245  if  (mode  ==  5){  /*  rotate  case  */ 

246  switch(mode5state){ 

247  case  0: 

248  /*  turn  each  wheel  by  +-PI/4  in  5  seconds  */ 

249  desiredAngles[0]  +=  QPIby500;  /*  =  (PI/4)/500  */ 

250  desiredAngles[  1  ]  -=  QPIby500; 

251  desiredAngles[2]  -=  QPIby500; 

252  desiredAngles[3]  +=  QPIby500; 

253  if  (desiredAngles[0]  >=  QPI) 

254  mode5  state  =  1 ; 

255  break; 
256 

257  case  1 :  /*  drive  wheels  to  rotate  body  */ 

258  desiredSpeeds[0]  =  +speed; 

259  desiredSpeeds[  1  ]  =  -speed; 

260  desiredSpeeds[2]  =  +speed; 
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261  desiredSpeeds[3]  =  -speed; 

262  break; 

263  } 

264  return; 

265  } 
266 

267  for  (i=0;  i  <  4;  i++){      /*  non-rotate  case  */ 

268  ro=whp[i].rho; 

269  ro2=ro*ro; 

270  beta=vehicle.heading+whp[i]. alpha; 

271  vlx  =  speed*cos(theta)-(whp[i].rho*omega*sin(beta)); 

272  vly  =  speed*sin(theta)+(whp[i].rho*omega*cos(beta)); 

273  desiredSpeeds[i]  =  new_sqrt(vlx*vlx  +  vly*vly); 
274 

275  switch(mode){ 

276  case  1: 

277  case  2: 

278  case  3; 

279  if  (speed  <  0.0) 

280  desiredSpeeds[i]  =  -desiredSpeeds[i]; 

281  if(new_abs(vlx)>0.01){ 

282  vlyvlxRatio=vly/vlx; 
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283  desiredAngles[i]  =  atan(vlyvlxRatio)  -  vehicle.heading; 

284  wheel AngleV  =  motion.Theta  -  vehicle.heading  -  whp[i]. alpha; 

285  desiredAngleRates[i]  = 

286  ( (speed*speed*thetaDot  +  ro2*Omega3) 

287  +speed*ro*omega*(omega+thetaDot)*sin(wheelAngleV) 

288  +ro*(omegaDot*speed-omega*speedDot)*cos(wheelAngleV) ) 

289  /(  desiredSpeeds[i]*  desiredSpeeds[i])  -  omega; 

290  desiredAnglesO[i]  =  desiredAngles[i]; 

291  desiredAngleRatesO[i]  =  desiredAngleRates[i]; 

292  } 

293  else{ 

294  desiredAngles[i]  =  desired  Angles0[i]; 

295  desiredAngleRatesfi]  =  desiredAngleRatesO[i]; 

296  } 

297  break; 
298 

299  case  4: 

300  if  (speed  <  0.0) 

301  desiredSpeedsp]  =  -desiredSpeeds[i]; 

302  if  (new_abs(vlx)>0.01){ 

303  v  1  y  v  1  xRatio= v  1  y/v  1  x ; 

304  desiredAnglesp]  =  theta; 
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305  desired  AngleRates[i]  =  0.0; 

306  desiredAnglesO[i]  =  desiredAnglesfi]; 

307  desiredAngleRatesO[i]  =  desiredAngleRates[i]; 

308  } 

309  else{ 

310  desiredAngles[i]  =  desiredAnglesO[i]; 

311  desiredAngleRates[i]  =  desiredAngleRatesO[i]; 

312  } 

3 1 3  break; 
314 

315  case  6: 

316  case  7: 

317  case  8: 

318  case  9: 

319  if  (new_abs(desiredSpeeds[i])  >  0.01 ){ 

320  desiredAngles[i]  =  atan2(vly,vlx)  -  vehicle. heading; 

321  wheel AngleV  =  motion.Theta  -  vehicle.heading  -  whp[i]. alpha; 

322  desiredAngleRates[i]  = 

323  ( (speed*speed*thetaDot  +  ro2*Omega3) 

324  +speed*ro*omega*(omega+thetaDot)*sin(  wheel  AngleV) 

325  +ro*(omegaDot*speed-omega*speedDot)*cos(wheelAngleV) ) 

326  /(  desiredSpeeds[i]*  desiredSpeeds[i])  -  omega; 
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327  desiredAnglesO[i]  =  desiredAngles[i]; 

328  desiredAngleRatesO[i]  =  desiredAngleRates[i]; 

329  } 

330  else{ 

33 1  desired  Angles  [i]  =  desiredAnglesO[i]; 

332  desiredAngleRates[i]  =  desiredAngleRatesO[i]; 

333  } 

334  break; 
335 

336  case  10: 

337  desiredSpeeds[i]  =  speed  * 

(new_sqrt((ai[i]*  vehicle.  kappa)*(ai[i]*  vehicle,  kappa) 

338  +(l-bi[i]*  vehicle. kappa)*(l-bi[i]*vehicle. kappa))); 

339  if  (vehicle.kappa  !=  0.0){ 

340  desiredAngles[i]  =  atan2(bi[i],  (vehicle.  kappa-ai[i])); 

341  } 

342  else  {  desiredAngles[i]  =0.0;} 

343  desiredAngleRates[i]  =  0.0; 

344  break; 

345  case  100: 

346  break; 

347  }/*  end  switch  */ 
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*  * 


348    }/*  end  for  */ 

349} 

350 

351 

352 

353/* 

354  *  joystickMotionlnterface  ejm  19  June  97 

355  * */ 

356void  joystickMotionlnterfaceO 

357  { 

358  motion. Speed  =joyStick.y;    /*  convert  x-position  into  double  */ 

359  motion.Theta  =  joystick. x;    /*  convert  y-poistion  into  double  */ 

360  motion. Omega  =0.0;  /*motion. Omega  =  joyStick.w;  not  implemented  yet;*/ 
361} 

362 

363/* * 

364  *  tangentialMotion  * 

365  * */ 

366void  tangentialMotionO 

367  { 

368  double  deltaTheta,  deltax,  deltay,  Si,  totalDistance,  deltaDistance; 

369  int  ix; 
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370 

371  deltax  =  0; 

372  deltay  =  0; 

373  for(ix  =  0;ix<4;ix++){ 

374  deltax  =  deltax  +  actualSpeeds[ix]*cos(actualAngles[ix]); 

375  deltay  =  deltay  +  actualSpeeds[ix]*sin(actualAngles[ix]); 

376  } 
377 

378  /*retums  the  linear  distance  the  vehicle  has  travelled  */ 

379  deltaS  =  (DeltaT/4)*new_sqrt((deltax*deltax)+(deltay*deltay)); 
380 

38 1  /*  returns  the  difference  between  the  changes  in  the  distance  */ 

382  /*  of  the  left  and  right  wheels  */ 

383  deltaTheta  =  0.0; 

384  for  (ix  =  0;  ix  <4;  ix++){ 

385  Si  =  actualSpeeds[ix]*DeltaT; 

386  deltaTheta  =  deltaTheta  +  (sin(actualAngles[ix])/ai[ix] 

387  -  cos(actualAngleslix])/bi[ix])*Si; 

388  } 

389  deltaTheta  =  deltaTheta/4; 
390 

391  totalDistance  +=  deltaS;  /*  Keeps  track  of  the  total  distance  traved  by  vehicle  */ 
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392 

393  /*  update  the  vehicle's  configuration  based  on  the  distance  travelled  */ 

394  /*  during  the  last  motion  control  cycle  */ 
395 

396  vehicle. heading  +=  deltaTheta; 

397  circularArc(deltaS,  deltaTheta); 

398  compose(); 
399 

400    deltaDistance  =  DeltaT*motion. Speed; 

401/*    vehicle. kappa  +=  (steer()*deltaDistance);*/ 

402    vehicle. kappa  =  0.0; 

403/*    motion. Theta +=  deltaTheta;  */ 

404/*    motion.Theta  =  vehicle.heading;  */ 

405    motion.Theta  =  0.0; 

406/*    motion. Omega  =  vehicle. kappa*motion. Speed;    */ 

407  motion. Omega  =  0.0; 

408  thetaDot  =  deltaTheta/DeltaT; 

409  speedDot  =  0.0; 

410  omegaDot  =  0.0; 
411} 

412 
413/*******************************************************************/ 
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414/*  FUNCTION:    circularArc()  */ 

415/*  PARAMETERS:  Configuration  length  -the  arc  length  */ 

416/*  alpha    --the  end  orientation  */ 

417/*  config  —pointer  to  the  resultant  configuration    */ 

418/*  PURPOSE:     Given  the  arc  length  and  alpha,  to  calculate  the  final  */ 

419/*  configuration  */ 

420/*  RETURNS:     Configuration:  pointer  to  the  final  configuration  */ 

421/*  COMMENTS:    The  main  purpose  of  this  function  is  to  be  used  in  conjunction  */ 

422/*  with  compose()  to  form  a  new  next().  In  this  case,  length  would*/ 

423/*  actually  be  delta-s  and  alpha  would  be  delta-theta.  */ 

424/*  Circular_arc()  would  determine  the  configuration  after  the  incre-*/ 

425/*  mental  move  in  the  local  coordinate  system  of  the  original  */ 

426/*  configuration.  Then  compose()  would  take  the  original  */ 

427/*  configuration  (in  global  coordinates)  and  the  incremental  */ 

428/*  configuration  (in  local  coordinates)  to  determine  the  */ 

429/*  incremental  configuration  in  global  coordinates.  */ 

430/*********************************************************************/ 

431  void 

432circularArc(double  length,  double  alpha) 

433  { 

434 

435  double  alpha2,alpha4; 
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436 

437  alpha2=  alpha*alpha; 

438  alpha4=  alpha2*alpha2; 

439  defineConfig((l-  alpha2/6.0  +  alpha4/ 120.0)  *  length, 

440  (0.5  -  alpha2/24  +  alpha4/720.0)  *  alpha  *  length, 

441  alpha,  0.0); 
442} 

444/*  FUNCTION:    defineConfigO  */ 

445/*  PARAMETERS:  double  x,y,theta,kappa        —The  values  that  define  a        */ 
446/*  configuration  */ 

447/*  PURPOSE:     To  allocate  nad  assign  a  configuration  */ 

448/*  RETURNS:     Configuration:     a  configuration  */ 

449/*  COMMENTS:    Was  called  def_configuration()  in  MML 10  */ 

450/*****************************************************************/ 
451  void 
452defineConfig(double  x,double  y,double  theta,double  kappa) 

453  { 

454  incrementalMotion.coord.x  =  x; 

455  incrementalMotion. coord. y  =  y; 

456  incrementalMotion. heading  =  theta; 

457  incrementalMotion. kappa  =  kappa; 
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458} 
459 

460 

46  ]/********************************************************************/ 

462/*  FUNCTION:    compose()  */ 

463/*  PARAMETERS:  Configuration  *first  —  pointer  to  the  first  configuration  */ 

464/*  *second  —  pointer  to  the  second  configuration  */ 

465/*  PURPOSE:     To  calculate  the  composition  of  the  first  and  second  */ 

466/*  configurations  */ 

467/*  RETURNS:     Configuration:  configuration  which  is  the  */ 

468/*  composition  of  the  first  and  second  configurations  */ 

469/*  COMMENTS:    A  typical  example  of  the  usage  of  this  function  is  to  determine  */ 

470/*  the  goal  position  of  a  configuration  in  global  coordinates.  In  */ 

471/*  such  an  example,  the  first  argument  would  be  the  original  */ 

472/*  configuration  and  the  second  argument  would  be  the  goal  */ 

473/*  configuration  in  the  original  configuration's  local  coordinate       */ 

474/*  system.  The  resultant  third  argument  would  then  be  the  goal       */ 

475/*  configuration  in  global  coordinates. Was  called  compQ  in  MML10  */ 

476/*  LAST  UPDATE:  10/25/94  Chien-Liang  Chuang  */ 

477/*******************************************************************/ 

478void 

479compose() 
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480  { 
481 

482  double  x,y,  theta; 

483  double  xx,yy,tt; 
484 

485  holdVehicle.coord.x  =  vehicle.coord.x; 

486  holdVehicle.coord.y  =  vehicle.coord.y; 

487  holdVehicle.heading  =  vehicle.heading; 

488  holdVehicle.kappa    =  vehicle. kappa; 
489 

490  x  =  incrementalMotion.coord.x; 

491  y  =  incrementalMotion.coord.y; 

492  theta  =  holdVehicle.heading; 
493 

494 

495  xx  =  cos(theta)  *  x  -  sin(theta)  *  y  +  holdVehicle.coord.x; 

496  yy  =  sin(theta)  *  x  +  cos(theta)  *  y  +  holdVehicle.coord.y; 
497 

498  tt  =  holdVehicle.heading  +  incrementalMotion. heading; 
499 

500  vehicle.coord.x  =  xx; 

501  vehicle.coord.y  =  yy; 
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502  vehicle. heading  =  tt; 

503  vehicle.kappa  =  holdVehicle. kappa; 
504} 

505 

507/*    FUNCTION  :  steer(robot,line)  PURPOSE  :  evaluate  steering  */ 

508/*    function  */ 

510double  steer() 

511 

512{ 

513  double  lambda,  angle,  dist; 

514 

515  if  (currentPath.config. kappa  ==  0.0) 

516  lambda  =  -  currentPath.a  *  vehicle.kappa 

517  -  currentPath.b  *  norm(vehicle. heading  -  currentPath.config. heading) 

518  -  currentPath.c  *(-(vehicle. coord. x  -  currentPath.config.coord.x) 

519  *  sin(currentPath.config. heading) 

520  +(vehicle.coord.y  -  currentPath.config.coord.y) 

521  *  cos(currentPath.config. heading)); 

522  else 

523  { 
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524  angle  =  Psi(vehicle.coord,  currentPath. center); 

525  dist  =  distance(currentPath.center,  vehicle.coord); 

526  if  (currentPath.config.kappa  >  0.0) 

527  { 

528  lambda  =  -  currentPath. a  *  (vehicle.kappa-currentPath.config.kappa) 

529  -  currentPath.b  *  norm(vehicle.heading-(angle-HPI)) 

530  -  currentPath.c  *  (currentPath. radius  -  dist); 

531  } 

532  else 

533  lambda  =  -  currentPath. a  *  (vehicle. kappa-currentPath.config. kappa) 

534  -  currentPath.b  *  norm(vehicle.heading-(angle+HPI)) 

535  -  currentPath.c  *  (currentPath. radius  +  dist); 

536  } 

537  return  lambda; 
538} 

539 
540/**************************************************************/ 

541  void  constantsQ 

542  { 

543  double  k; 
544 

545    k  =  1 .0/sigma; 
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546  currentPath.a  =  3.0*k; 

547  currentPath.b  =  3.0*k*k; 

548  currentPath.c  =  k*k*k; 
549} 

551/*  Function:  Psi_function()  */ 

552/*  Purpose:  Computes  the  Psi  function  of  two  given  points  */ 

553/*  Parameters:  point  pi, p2  */ 

554/*  Returns:  double  */ 

555/*  Comments:  */ 

556/***************************************************************/ 
557double 

558Psi(point  pi, point  p2) 
559 

560  { 

561  if  (  p2.y  -  p  1  .y  ==  0.0  &&  p2.x  -  p  1  .x  ==  0.0) 

562  return  0.0; 

563  else 

564  return  atan2(p2.y  -  pl.y,  p2.x  -  p  1  .x); 
565} 

566 
567 
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568/*******************************************************************/ 
569/*  Function:  distance()  */ 

570/*  Purpose:  Computes  the  distance  between  two  given  points  */ 

571/*  Parameters:  point  pi, p2  */ 

572/*  Returns:  double  */ 

573/*  Comments:  */ 

574/**************************************************************** 

575double 

576distance(point  pi, point  p2) 

577 

578  { 

579  double  X,  Y; 
580 

581  X  =  pl.x-p2.x; 

582  Y  =  pl.y-p2.y; 

583  return  new_sqrt(  X*X  +  Y*Y  ); 
584} 

585 

586void  initTangent() 

587  { 

588  currentPath.config. coord. x  =  0.0; 

589  currentPath.config.coord.y  =  0.0; 
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590  currentPath.config.heading  =  0.0; 

591  currentPath.config.kappa  =  0.0; 

592  currentPath.radius  =  0.0; 

593  currentPath.center.x  =  0.0; 

594  currentPath.center.y  =  0.0; 

595  sigma  =  20.0; 

596  constants(); 
597 

598  motion.Speed  =  10.0; 

599  motion.Theta  =  0.0; 

600  motion. Omega  =  0.0; 
601 

602  vehicle.coord.y  =  0.0; 

603  vehicle.coord.x  =  0.0; 
604 

605 

606} 

607 
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APPENDIX  E:  SOURCE  CODE  (MOTOR.C) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 

1  /*============================================================= 

2  //  Edward  Mays 

3  //  Shpeherd  project 

4  // 20  February  1997 

5  //  MotionControl 

6  //============================================================*/ 

7 

8  #include  "motor.h" 

9  #include  "shepherd. h" 
10#include"math.h" 

11 

1 2  void  readEncoders()  { 

1 3  readDriveEncoders(driveReadings); 

14  readSteerEncoders(steerReadings); 
15} 

16 

17/*  */ 

18  /*  Verifies  validity  of  incoming  speeds/angles  and  converts     */ 

19  /*  digitial  input  for  the  DA  board  */ 

20  /*  */ 

21  void  driveMotors(){ 
22 

23      int  ix,Speed_Digit,Steer_Digit,  counter; 
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24  double  speed  1 ,  steer  1 ,  temp ; 
25 

26  unsigned  short  bitMask=0x8000;       /*  access  bit  15  for  align  wheel  1  */ 

27  unsigned  short  *servoStatus=(unsigned  short  *)(VME9421+0x00ca);  /*  digital  input  */ 
28 

29  bitMask  =  bitMask  »  3; 
30 

31  /*  updateWheelDriveO;  wheel  values  for  driving  */ 

32  /*  updateWheelSteer();  */ 

33  /*  comupte  the  current  actual  wheel  direction  in  WheelDirAct[]  */ 
34 

35  if  (mode  !=  100)  { 

36  for(ix  =0;  ix  <ARRAY_SIZE;  ix++)  { 

38  /*  here  +/-  1/50  of  the  steering  value  is  added  to  the  driving        */ 

39  /*  for  each  specified  wheel.  Note  the  negative  sign  on  elements  [1]  */ 

40  /*  and  [3]provide  the  same  direction  driving  as  elements  [0]  and  [2]  */ 
41 

42  Omega_Speed  =  desiredSpeedspx]  + 

43  SteerDriveInteract*desiredAngleRates[ix]*WheelRadius;  /*  cm/sec  */ 
44 

45  /*  conversion  to  digits    */ 

46  Speed_Digit  =  velocityReferenceTable(Omega_Speed,ix)  + 

47  DriveFeedBackGain*(Omega_Speed  -  actualSpeeds[ix]); 

48  Steer_Digit  =  rateReferenceTable(desiredAngleRates[ix]) 

49  +  steerFeedbackGain*(desiredAngleRateslixl-actualAngleRates[ix]) 
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50  +  angleFeedbackGain*norm(desiredAngles[ix]-actualAngles[ix]); 
51 

52  if  (Speed_Digit>DigitsHigh)  /*  Limitation  */ 

53  Speed_Digit=  DigitsHigh; 

54  if  (Steer_Digit>DigitsHigh) 

55  Steer_Digit=  DigitsHigh; 

56  if  (Speed_Digit<DigitsLow) 

57  Speed_Digit=  DigitsLow; 

58  if  (Steer_Digit<DigitsLow) 

59  Steer_Digit=  DigitsLow; 
60 

61  switch(mode){ 

62  case  2: 

63  case  3: 

64  case  4: 

65  case  5: 

66  case  6: 

67  case  7: 

68  case  8: 

69  case  9: 

70  case  10: 

71  Speed_Digits[ix]=  (short)Speed_Digit;  /*  casting  to  short  */ 

72  Steer_Digits[ix]=  (short)Steer_Digit; 

73  break; 
74 

75  case  1: 
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76 

speed  1  =  Speed_Digits[ix]; 

77 

steerl  =  Steer_Digits[ix]; 

78 

if  (  speed  1  >  0)  speed  1--; 

79 

if  (  speed  1  <  0)  speed  1++; 

80 

if  (  steerl  >  0)  steerl--; 

81 

if  (  steerl  <  0)  steer  1++; 

82 

Speed_Digits[ix]  =  speedl; 

83 

Steer_Digits[ix]  =  steerl; 

84 

break; 

85 

}  /*  end  switch  */ 

86 

}  /*  end  for  */ 

87 

}  /*  end  if  */ 

88 

else  { 

89 

for  (ix=0;  ix<3;  ix++){ 

90 

Steer_Digits[ix]  =  0; 

91 

} 

92 

for  (ix=0;  ix<4;  ix++){ 

93 

Speed_Digits[ix]  =  0; 

94 

} 

95 

96 

switch(modeTstate)  { 

97 

case  0: 

98 

Steer_Digits[3]  =  50*Flag; 

99 

modeTstate  =  1 ; 

100 

break; 

101 
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102 

case  1: 

103 

modeTstate  = 

=  2; 

104 

break; 

105 

106 

case  2: 

107 

modeTstate  = 

=  3; 

108 

break; 

109 

110 

case  3: 

111 

modeTstate  = 

=  4; 

112 

break; 

113 

114 

case  4: 

115 

modeTstate  = 

=  5; 

116 

break; 

117 

118 

case  5: 

119 

modeTstate  = 

=  6; 

120 

break; 

121 

122 

case  6: 

123 

modeTstate  = 

=  7; 

124 

break; 

125 

126 

127 

case  7: 
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128 

modeTstate  = 

8; 

129 

break; 

130 

131 

case  8: 

132 

modeTstate  = 

9; 

133 

break; 

134 

135 

case  9: 

136 

modeTstate  = 

10; 

137 

break; 

138 

139 

case  10: 

140 

modeTstate  = 

11; 

141 

break; 

142 

143 

case  1 1 : 

144 

modeTstate  = 

12; 

145 

break; 

146 

147 

case  12: 

148 

modeTstate  = 

13; 

149 

break; 

150 

151 

case  13: 

152 

modeTstate  = 

14; 

153 

break; 
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154 

155 

case  14: 

156 

modeTstate  =15; 

157 

break; 

158 

159 

case  15: 

160 

modeTstate  =16; 

161 

break; 

162 

163 

case  16: 

164 

modeTstate  =17; 

165 

break; 

166 

167 

case  17: 

168 

modeTstate  =18; 

169 

break; 

170 

171 

case  18: 

172 

modeTstate  =  19; 

173 

break; 

174 

175 

case  19: 

176 

if  (bitMask&*ser 

177 

{ 

178 

Flag  =  -Flag; 

179 

modeTstate  =  2 

/*wait  until  wheel  aligned  */ 
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180 

} 

181 

break; 

182 

183 

case  20: 

184 

Steer_Digits[3]  =  0; 

185 

modeTstate  =  21; 

186 

break; 

187 

188 

case  21: 

189 

modeTstate  =  22; 

190 

break; 

191 

192 

case  22: 

193 

modeTstate  =  23; 

194 

break; 

195 

196 

case  23: 

197 

modeTstate  =  24; 

198 

break; 

199 

200 

case  24: 

201 

modeTstate  =  25; 

202 

break; 

203 

204 

case  25: 

205 

modeTstate  =  26; 
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206  break; 

207 

208  case  26: 

209  modeTstate  =  27; 

210  break; 
211 

212  case  27: 

2 1 3  modeTstate  =  0; 

214  break; 
215 

216  default :  break; 

217  }  /*  end  switch  */ 

218  }/*  end  else*/ 
219 

220  driveSteer(Steer_Digits); 

221  driveSpeed(Speed_Digits); 

222  }/*  end  driveMotors  */ 
223 

224  /*  Wheel  Driving  function  */ 
225void  driveSpeed(short  Speed_Digits[])  { 
226 

227  unsigned  int    *servoControl=(unsigned  int  *)VME2170;  /*  Data  Out        */ 

228  short  *servoOutl=(unsigned  short* )(VME92 10+0x0082);     /*  Analog  out      */ 

229  short  *servoOut3=(unsigned  short* )(VME92 10+0x0086);     /*  Analog  out  test*/ 

230  short  *servoOut2=(unsigned  short* )(VME92 10+0x0084);     /*  Analog  out  test*/ 

231  short  *servoOut4=(unsigned  short*)(VME92 10+0x0088);     /*  Analog  out  test*/ 

139 


232 

233  unsigned  int  wheelSelect=0x00924924;  /*  select  all  wheels  for  driving  or  steering  */ 

234 

235  *servoControl=wheelSelect; 

236 

237  *servoOutl=  (-Speed_Digits[0])«4; 

238  *servoOut3=  (-Speed_Digits[2])«4; 

239  *servoOut2=    Speed_Digits[l]  «4; 

240  *servoOut4=    Speed_Digits[3]  «4; 
241 

242  return; 

243}  /*  driveSpeed*/ 

244 

245 

246 

247/*  Wheel  Steering  function  */ 

248 void  driveSteer(short  Steer_Digits[]){ 

249 

250  unsigned  int  *servoControl=(unsigned  int  *)VME2170;    /*  Data  Out  */ 

251  short  *servoOutl=(unsignedshort*)(VME92 10+0x008 A);    /*  Analog  out  wheel  1  */ 

252  short  *servoOut3=(unsigned  short*)(VME9210+0x008E);    /*  Analog  out  wheel3*/ 

253  short  *servoOut2=(unsigned  short*)(VME9210+0x008C);    /*  Analog  out  wheel2*/ 

254  short  *servoOut4=(unsigned  short*)(VME92 10+0x0090);    /*  Analog  out  wheel4*/ 
255 

256 
257 
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258  /*  select  all  wheels  for  driving  or  turning  */ 

259  unsigned  int  wheelSelect=0x00924924; 
260 


/*  a  neg  volt  turns  wheels  clockwise  */ 
/*  a  pos  volt  turns  counter  clockwise*/ 


261  *servoOutl=  Steer_Digits[0]«4 

262  *servoOut3=  Steer_Digits[2]«4 

263  *servoOut2=  Steer_Digits[l]«4 

264  *servoOut4=  Steer_Digits[3]«4 

265  *servoControl=wheelSelect;  /*  turn  on  selected  servo  motor  */ 
266 

267    return; 

268 

269 }  /*  end  of  driveSteer  */ 

270 

271 

272 

273/*  Wheel  stop  function  */ 

274 void  allStop(){ 

275 

276  unsigned  int    *servoControl=(unsigned  int  *)VME2170;  /*  Data  Out  */ 

277  /*  short  *servoOutl=(unsigned  short*)(VME92 10+0x0084);*/ 
278 

279    /*  deselect  all  wheels  for  driving  and/or  turning  */ 
280 

28 1  *servoControl=0x00000000;         /*  turn  off  selected  servo  motor  */ 

282  /*  *servoOutl=  0.0;*/  /*  temp,  does  not  belong  in  this  function  */ 

283  initBoards(); 
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284    return; 

285 

286}  /*  end  of  allStop  */ 

287 

288 

289void  wheelDrive() 

290  {  intix,a; 

291  double  alpha[ARRAY_SIZE]={30,  0,  0,  0},  beta[ARRAY_SIZE]={0.0,  0.0,  0.0,  0.0}; 
292 

293  driveMotors(alpha,beta); 

294 

295  return; 

296}/*end  wheelDrive  */ 

297 

298 

299 

300void  readDriveEncoders(unsigned  long  int  array[]) 

301  { 

302  unsigned  char  *p=(unsigned  char*)VMECTR  1 ,  c  1 ,  c2,  c3; 

303  int  ix; 

304  long  int  temp; 
305 

306    for  (ix=0;  ix<4;  ix++)  {    /*  read  all  four  motors  subsequentially  */ 
307 

308  *(p+3)=Ox03;  /*  load  output  latch  from  counter  */ 

309  *(p+3)=0x01;  /*  control  register,  initialize  two-bit  output  latch  */ 
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310 

311  /*  read  three  bytes  for  specific  counter  ix  and  save  in  status    */ 

312  /*  first  access  to  Output  Latch  Register  reads  least  significant  */ 

313  /*  byte  first  */ 
314 

315  cl  =  *(p+l)&0x00ff; 

316  c2  =  *(p+l)&0x00ff; 

317  c3  =  *(p+l)&0x00ff; 

3 1 8  array[ix]  =  ((unsigned  int)c  1  )l  ((unsigned  int)c2  «  8)  I 

319  ((unsigned  int)c3  «  16); 
320 

32 1       p=p+4;  /*  increment  pointer  for  next  counter  */ 

322 

323 

324  } 

325  return; 

326}  /*  end  of  readDriveEncoders  */ 

327 

328 

329int  readSteerEncoders(unsigned  long  int  array[]) 

330{ 

331  unsigned  char  *p=(unsignedchar*)(VMECTRl  +0x0100),  cl,c2,c3; 

332  int  ix; 
333 

334 

335    for  (ix=0;  ix<4;  ix++)  {    /*  read  all  four  motors  subsequentially  */ 
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336 

337  *(p+3)=0x03;  /*  load  output  latch  from  counter  */ 

338  *(p+3)=0x01;  /*  control  register,  initialize  two-bit  output  latch  */ 
339 

340 

341  /*  read  three  bytes  for  specific  counter  ix  and  save  in  status  */ 

342  /*  first  access  to  Output  Latch  Register  reads  least  significant  byte  first  */ 
343 

344  cl  =  *(p+l)&0x00ff; 

345  c2  =  *(p+l)&0x00ff; 

346  c3  =  *(p+l)&0x00ff; 

347  array  [ix]  =  ((unsigned  int)cl)l  ((unsigned  int)c2  «  8)  I 

348  ((unsigned  int)c3  «  16); 
349 

350 

35 1       p=p+4;  /*  increment  pointer  for  next  counter  */ 

352 

353  } 

354  return; 

355 }  /*  end  of  readSteerEncoders  */ 

356 

357void  displayDirections() 

358  { 

359  /*if(edCounter%10==0){         */ 

360  convertBCD(bcdString+9,(unsignedint)(WheelDirDes[2]*RadsToDegrees)); 

361  /*bcdString+9*/ 
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362  convertBCD(bcdString+9,(unsigned  int)edCounter); 

363  bcdString[3]='0'; 

364  bcdString[4]='3'; 

365  bcdString[6]='4'; 

366  bcdString[7]='0'; 

367  sioOut(0,bcdString); 
368 

369  /*  sioOut(0,clrLine);  */  /*  clear  line       */ 
370 

371  convertBCD(bcdString+9,(unsignedint)(WheelDirAct[2]*RadsToDegrees)); 

372  bcdString[3]='0'; 

373  bcdString[4]='3'; 

374  bcdString[6]='6'; 

375  bcdString[7]='0'; 

376  sioOut(0,bcdString); 
377 

378} 

379 

380/*  2  May  */ 

38 1  void  displaySpeed() 

382  { 
383 

384  convertBCD(bcdString+9,(unsigned  int)l); 

385  bcdString[3}='0'; 

386  bcdString[4]='3' 

387  bcdString[6]='4' 

145 


388  bcdString[7]='0*; 

389  sioOut(0,bcdString); 
390 

391        /*  sioOut(0,clrLine);  */  /*  clear  line       */ 
392 

393  convertBCD(bcdString+9,(unsigned  int)steerReadings[l]); 

394  bcdString[3]=0'; 

395  bcdString[4]='3'; 

396  bcdString[6]='6*; 

397  bcdString[7]=*0'; 

398  sioOut(0,bcdString); 
399} 

400 
401voidtestDrivel() 

402  { 

403  desiredAngleRates[0]  =  1; 

404  desiredAngleRates[l]  =   1; 

405  desiredAngleRates[2]  =  1 ; 

406  desiredAngleRates[3]  =   1 ; 

407  desiredSpeeds[0]  =  0;  /*  wheels  2&4  must  have  minus  sign  */ 

408  desiredSpeeds[l]  =  0;  /*  wheels  2&4  must  have  minus  sign  */ 

409  desiredSpeeds[2]  =  0;  /*  wheels  2&4  must  have  minus  sign  */ 

410  desiredSpeeds[31  =  0;  /*  wheels  2&4  must  have  minus  sign  */ 
411 

412} 
413 
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414/*  2  May*/ 

41 5  void  testDrive() 

416{ 

417  double  MM,  RES,  N=  1044548,  C=0.001; 

418 

419sioOut(0,"Entering  testDrive  ...\n\r"); 

420 

421 

422if(20<(N  -  steerReadings[l])*C){ 

423 

424  MM=20.0; 

425  } 

426  else  { 
427 

428  MM=(N  -  steerReadings[  1  ])*C; 

429  } 
430 
431 

432  if(MM>0){ 
433 

434  RES=MM; 

435  } 

436  else{ 
437 

438  RES=0.0; 

439  } 
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440 

441 

442 

443  /*  RES=max(min(20,(N  -  steerReadings[0])*C),0);  */ 

444 

445sioOut(0,"Leaving  testDrive  ...\n\r"); 

446  desiredSpeeds[l]  =  -(RES);  /*  wheels  2&4  must  have  minus  sign  */ 

447 

448  return; 

449} 

450 

451 

452 

453 

454 

455void  computeActualRates() 

456  { 

457 

458inti; 

459double  count,speed; 

460 

461  for(i=0;  i<=3;  i++) 

462  { 

463  if(PreviousCountSpeed[i]  ==  99999999)  /*  for  derivative  for  speed  */ 

464  actualSpeeds[i]=  0.0; 

465  else 
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466  actualSpeeds[i]= 

467  (convertDifference((driveReadings[i]  -  PreviousCountSpeedfi])) 

468  *DigitToCmDrive[i])/DeltaT; 

469  PreviousCountSpeed[i]  =  driveReadings[i]; 
470 

471  if(PreviousCountSteer[i]  =  99999999)  /*  for  derivative  for  steering  */ 

472  actualAngleRates[i]=  0.0; 

473  else 

474  actualAngleRates[i]= 

475  (convertDifference((steerReadings[i]  -  PreviousCountSteer[i])) 

476  *digitToRadSteer)/DeltaT; 

477  PreviousCountSteer[i]  =  steerReadings[i]; 

478  } 
479} 
480 
481 
482 

483void  accumulateDriveSpeed() 

484  { 

485  int  i; 
486 
487for(i=0;i<=3;i++){ 

488  Display_Speeds[i]  +=  actualSpeeds[i]; 

489  } 

490  return; 
491} 
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492 

493  void  accumulateDriveSteer() 

494  { 

495  int  i; 
496 
497for(i=0;i<=3;i++){ 

498  Display_Steers[i]  +=  10*actualAngleRates[i]; 

499  actual Angles[i]  +=  actualAngleRates[i]*DeltaT; 
500} 

501  return; 

502} 

503 

504/*  added  15  may  */ 

505 void  displayDriveAngle() 

506  { 

507 

508  double  angle,  angle  1,  angle2,  angle3; 

509  angle  =  actual AngleRates[0]  *  1000.0; 

510  angle  1  =  actual AngleRates[l]  *  1000.0; 

511  angle2  =  actual AngleRates[2]  *  1000.0; 

512  angle3  =  actual AngleRates[3]  *  1000.0; 
513 

514  if  (edCounter%100==0){ 

5 1 5  convertInt(bcdString+9,(int)desiredAngleRates[0]); 

516  bcdString[3]='0'; 

517  bcdString[4]=3'; 
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518  bcdString[6]='4'; 

519  bcdString[7]='0'; 

520  sioOut(0,bcdString); 

521  convertInt(bcdString+9,(int)  angle); 

522  bcdString[3]='0'; 

523  bcdString[4]='3'; 

524  bcdString[6]='6'; 

525  bcdString[7]=*0'; 

526  sioOut(0,bcdString); 

527  convertInt(bcdString+9,(int)  angle  1); 

528  bcdString[3]='0'; 

529  bcdString[4]='4'; 

530  bcdString[6]='6'; 

531  bcdString[7]='0'; 

532  sioOut(0,bcdString); 

533  convertInt(bcdString+9,(int)  angle2); 

534  bcdString[3]='0'; 

535  bcdString[4]='5'; 

536  bcdString[6]='6'; 

537  bcdString[7]='0'; 

538  sioOut(0,bcdString); 

539  convertInt(bcdString+9,(int)  angle3); 

540  bcdString[3]='0'; 

541  bcdString[4]='6'; 

542  bcdString[6]='6'; 

543  bcdString[7]='0'; 
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544        sioOut(0,bcdString) ; } 

545 

546  return; 

547} 

548 

549 

550 

55 1  double  velocityReferenceTable(double  desiredVelocity,int  i) 

552  { 

553  double  in  Velocity, 

554  outVelocity; 
555 

556    inVelocity=new_abs(desiredVelocity); 
557 

558  if  (inVelocity>=0.0  &&  in  Velocity  <=5.0) 

559  outVelocity  =  inVelocity*Kl  [i]; 
560 

561  if  (inVelocity>5.0  &&  inVelocity<  8.0) 

562  outVelocity  =  inVelocity*K2[i]; 
563 

564  if  (inVelocity>=8.0  &&  inVelocity<20.0) 

565  outVelocity  =  inVelocity*K3[il; 
566 

567  if  (inVelocity>=20.0  &&  inVelocity<=  70.0) 

568  outVelocity  =  inVelocity*K4[i]; 
569 
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570  if  (inVelocity>70.0  &&  inVelocity<K5) 

571  outVelocity  =  inVelocity*K6[i]; 
572 

573  if  (inVelocity>  K5) 

574  outVelocity=1023; 
575 

576  if  (desiredVelocity<  0.0) 

577  outVelocity  =  -  outVelocity; 
578 

579  return  outVelocity; 

580}  /*  end  velocityLookupTable  */ 

581 

582 

583double  rateReferenceTable(double  desiredRate) 

584  { 

585  double  inRate, 

586  outDigit; 
587 

588  /*outDigit  =  new_abs(desiredRate);  *//*  test  only  */ 

589 

590  inRate=new_abs(desiredRate); 

591 

592  if  (inRate<=  5.234) 

593  outDigit  =  inRate*  195.41 55  ; 

594  else 

595  outDigit=1023; 
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596 
597 

598  if  (desiredRate<  0.0) 

599  outDigit  =  -  outDigit; 
600 

601    return  outDigit; 

602} 

603 

604 

605 

607  Function  convertDifference()  returns  the  difference  between  the  new  shaft 

608  encoder  position  and  the  old  shaft  encoder  position.  The  shaft  encoder  values 

609  contain  only  24  bits  (OxOOOOOO-Oxffffff).  The  routine  adjusts  for  the  trans- 

6 1 0  ition  from  Oxffffff  to  0x000000  and  vice  versa. 

612 

613int  convertDifference(int  value) 

614{ 

615  if( value  <  -0x800000) 

616  value  &=  OxOOffffff; 

617  else  if( value  >=  0x800000) 

618  value  1=  OxffOOOOOO; 
619 

620    return  value; 
621} 
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622 

623/* * 

624  *  * 

625  *  File:  S  E  R  V  O  .  C  * 

626  *  * 

627  *  Environment:  GCC  Compiler  v2.7.2  * 

628  *  Last  update:  30  January  1997  * 

629  *  Name:  Thorsten  Leonardy  * 

630  *  Purpose:       Provides  the  kernel  for  SHEPHERD.  * 

631  *  * 

632  *  Compiled:      >gcc  -c  -m68040  -o  servo. o  servo.c  * 

633  *  * 

634  * */ 

635 

636 
637 
638/* * 

639  *  readWheelStatus()  * 

640  *  * 

641  *  Environment:  GCC  Compiler  v2.7. 2  * 

642  *  Last  update:  20  February  1997  * 

643  *  Name:  Thorsten  Leonardy  * 

644  *  Purpose:       This  function  reads  the  wheels  counter  status.         * 

645  *  This  routine  makes  use  of  the  fact  that  arrays  are  stored    * 

646  *  in  memory  consecutively.  * 

647  *  * 
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648  *  array  points  to  the  beginning  of  the  array  'wheelEncoder'.         * 

649  * */ 

650  void  readWheelStatus(unsigned  char  *  array) 

651  { 

652  unsigned  char  *p=(unsigned  char*)VMECTRl ; 

653  int  ix; 
654 

655  for  (ix=0;  ix<8;  ix++)  {    /*  read  all  eight  motors  subsequentially  */ 
656 

657  *(p+3)=Ox03;  /*  load  output  latch  from  counter  */ 

658  *(p+3)=0x01;  /*  control  register,  initialize  two-bit  output  latch  */ 
659 

660  /*  read  three  bytes  for  specific  counter  ix  and  save  in  status  */ 

661  /*  first  access  to  Output  Latch  Register  reads  least  significant  byte  first  */ 

662  *(array+3)=*(p+l); 

663  *(array+2)=*(p+l); 

664  *(array+l)=*(p+l); 

665  *(array+0)=0; 
666 

667  array+=4; 

668  p=p+4; 
669 

670  if  (ix==3)  p=(unsigned  char*)VMECTR2;  /*  access  the  second  VME  Counter  */ 
671 

672  } 

673  return; 


/*  point  to  next  entry  in  wheelEncoder*/ 
/*  increment  pointer  for  next  counter  */ 
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674 }  /*  end  of  readWheel  Status  */ 

675 

676 

677/* * 

678  *  clearShaftEncoder(unsigned  short  motors)  * 

679  *  * 

680  *  Environment:  GCC  Compiler  v2.7.2  * 

681  *  Last  update:  04  March  1997  * 

682  *  Name:  Thorsten  Leonardy  * 

683  *  Purpose:       This  function  clears  the  selected  shaft  encoder.     * 
684*  * 

685  *  motors         bit  mask  to  select  motors,  eg.  0x042  selects  motor  2  and  7  * 

686  *  to  be  cleared.  * 

687  * */ 

688void  clearShaftEncoder(unsigned  short  motors) 

689  { 

690  unsigned  char  *p=(unsigned  char*)VMECTRl ; 

691  int  ix; 
692 

693  for  (ix=0;  ix<8;  ix++,motors/=2)  { 

694  if  (motors  &  0x01)  *(p+3)=0x04;         /*  clear  respective  counter  */ 

695  p=p+4;  /*  access  next  pointer       */ 

696  if  (ix=3)  p=(unsigned  char*)VMECTR2;  /*  access  the  second  VME  Counter  */ 

697  } 

698  return; 

699 }  /*  end  of  clearShaftEncoder  */ 
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700 
701 
702/* * 

703  *  align()  * 

704  *  Environment:  GCC  Compiler  * 

705  *  Last  update:  07  August  1997  m  * 

706  *  Name:  Thorsten  Leonardy  ,Yutaka  Kanayama,  Ed  Mays  * 

707  *  Purpose:       This  function  will  align  SHEPHERD'S  wheels  such  that  all     * 

708  *  will  point  in  the  forward  direction.  It  utilizes  the  hall    * 

709  *  sensors  for  each  of  the  four  wheels.  Crucial  parameters      * 
710*  are  as  follows:  * 

71 1  *  * 

712  * */ 

713void  align(void) 

714{ 

715  unsigned  int    *servoControl=(unsigned  int  *)VME2170;  /*  Data  Out  */ 

716  unsigned  short  *servoOut=(unsigned  short*)(VME9210+0x008A);      /*  Analog  out  */ 

717  unsigned  short  *servoStatus=(unsigned  short  *)(VME9421+0x00ca);  /*  digital  input  */ 

718  unsigned  short  bitMask=0x8000,  bitMaskl;        /*  access  bit  15  for  align  wheel  1  */ 

719  unsigned  int  wheelSelect=0x00004000;  /*  select  servo  for  turning  wheel  1  */ 

720  int  ix,  not  Yet;  /*  just  a  counter  */ 
721 

722  do  { 

723  not  Yet  =  0; 

724  bitMaskl  =  bitMask; 

725  for  (ix=0;  ix  <  4;  ix++) 
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if  (bitMaskl  &  *servoStatus) 

{ 
Steer_Digits[ix]  =  0; 

} 
else 

{ 

Steer_Digits[ix]  =  40; 
notYet++; 

} 
bitMaskl  =  bitMaskl  »  1;       /*  select  next  status  align  bit     */ 


726    { 

727 

728 

729 

730 

731 

732 

733 

734 

735 

736 

737  } 

738  driveSteer(Steer_Digits); 

739  }  while(notYet); 

740  *servoControl=0x00000000; 

741  return; 

742 }  /*  end  of  align  */ 

743 

744/* * 

745  *  alignAfterRotate()  * 

746  *  Environment:  GCC  Compiler  * 

747  *  Last  update:  07  August  1997  m  * 

748  *  Name:  Thorsten  Leonardy,Yutaka  Kanayama,  and  Ed  Mays  * 

749  *  Purpose:       This  function  will  align  SHEPHERD'S  wheels  such  that  all    * 

750  *  will  point  in  the  forward  direction.  It  utilizes  the  hall  * 

75 1  *  sensors  for  each  of  the  four  wheels.  Crucial  parameters         * 


/*  disable  all  wheels 


*/ 
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752  *  are  as  follows:  * 

753  *  * 

754  * */ 

755 void  alignAfterRotate(void) 

756  { 

757  unsigned  int    *servoControl=(unsigned  int  *)VME2170;  /*  Data  Out  */ 

758  unsigned  short  *servoOut=(unsigned  short*)(VME92 10+0x008 A);      /*  Analog  out  */ 

759  unsigned  short  *servoStatus=(unsigned  short  *)(VME9421+0x00ca);  /*  digital  input  */ 

760  unsigned  short  bitMask=0x8000,  bitMaskl;        /*  access  bit  15  for  align  wheel  1  */ 

761  unsigned  int  wheelSelect=0x00004000;  /*  select  servo  for  turning  wheel  1  */ 

762  int  ix,  not  Yet;  /*  just  a  counter  */ 
763 

764  do  { 

765  not  Yet  =  0; 

766  bitMaskl  =  bitMask; 

767  for  (ix=0;  ix  <  4;  ix++) 

768  { 

769 

770 

771 

772 


if  (bitMaskl  &  *servoStatus) 

{ 
Steer_Digits[ix]  =  0; 

} 


773  else 

774  { 

775  if(  ix=l  II  ix=2 ) 

776  Steer_Digits[ix]  =  40;    /*  for  wheel  1  and  2,  rotate  CCW  */ 

777  else 
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778  Steer_Digits[ix]  =  -40;  /*  for  wheel  0  and  3,  rotate  CW  */ 

779  notYet++; 

780  } 

781  bitMaskl  =  bitMaskl  »  1;       /*  select  next  status  align  bit     */ 

782  } 

783  driveSteer(Steer_Digits); 

784  }  while(notYet); 

785  *servoControl=0x00000000;  /*  disable  all  wheels  */ 

786  return; 

787}  /*  end  of  align  */ 

788 

789 

790/* * 

791  *  align Wheels()  * 

792  *  Environment:  GCC  Compiler  * 

793  *  Last  update:  07  January  1997  * 

794  *  Name:  Thorsten  Leonardy  * 

795  *  Purpose:       This  function  will  align  SHEPHERD'S  wheels  such  that  all 

796  *  will  point  in  the  forward  direction.  It  utilizes  the  hall    * 

797  *  sensors  for  each  of  the  four  wheels.  Crucial  parameters      * 

798  *  are  as  follows:  * 

799  *  * 

800  *  servoControl  Base  address  for  the  channels  controling  the  servo  motors 

801  *  switch  servos  on  an  off  by  accessing  this  address.  * 

802  *  Each  servo  is  controlled  by  three  bits:  * 

803  *  bits  0..2  ->  driving  wheel  1  * 

161 


804  *  3..5  ->  driving  wheel  2                              * 

805  *  6.. 8  ->  driving  wheel  3                              * 

806  *  9..1 1  ->  driving  wheel  4                              * 
807*  12..14  ->  turning  wheel  1                                * 
808*  15..17  ->  turning  wheel  2                              * 
809  *  18..20  ->  turning  wheel  3                              * 
810*  21..23  ->  turning  wheel  4                              * 

811  *  24..31  ->  not  used                                     * 

812  *  * 

813  *  servoOut       Base  address  for  the  analog  output  card  controlling  the 

814  *  speed  of  the  servos.  Only  the  highest  12  bits  are  used.      * 
815*  0x0010 ->  selects  lowest  positive  velocity                  * 

816  *  0x7ff0  ->  selects  highest  positive  velocity                  * 

817  *  OxfffO  ->  selects  lowest  negative  velocity  (i.e  -1  m/s)      * 

818  *  0x8000  ->  selects  highest  negative  velocity  (i.e.  -lOOOm/s)  * 

819  *  * 

820  *  It  has  been  found  that  the  MSB  does  not  work  properly.       * 

821  *  Therefore,  the  velocities  should  lie  within  1 1  bit  range,    * 
822*  -1024  <=  velocity  <=  +1023                                 * 

823  *  * 

824  *  servoStatus    Base  address  for  reading  the  servo  status  * 

825  *  The  alignment  bits  are:  Port  B,  Bit  15  for  wheel  1           * 
826*  Bit  14  for  wheel  2           * 

827*  Bit  13  for  wheel  3           * 

828*  Bit  12  for  wheel  4           * 

829  *  * 
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830  * */ 

831 
832 
833void  alignWheels(void) 

834  { 

835  unsigned  int    *servoControl=(unsigned  int  *)VME2170;  /*  Data  Out  */ 

836  unsigned  short  *servoOut=(unsigned  short*)(VME92 10+0x008 A);      /*  Analog  out  */ 

837  unsigned  short  *servoStatus=(unsigned  short  *)(VME9421+0x00ca);  /*  digital  input  */ 
838 

839  unsigned  short  bitMask=0x8000;       /*  access  bit  15  for  align  wheel  1  */ 

840  unsigned  int  wheelSelect=0x00004000;  /*  select  servo  for  turning  wheel  1  */ 

841  int  wheel;  /*  just  a  counter  */ 
842 

843  /* * 

844  *  align  wheels  subsequentially,  start  with  wheel  1  (front  right)  * 

845  * */ 

846  for  (wheel=l;  wheel<5;  wheel++)  { 
847 

848  *servoOut++=0x0200;  /*  set  output  value  for  servo  first             */ 

849  /*  0x0010  corresponds  to  smallest  velocity      */ 

850  *servoControl=wheelSelect;        /*  turn  on  selected  servo  motor  */ 

851  while(!(bitMask&*servoStatus));    /*  read  servo  status,  wait  until  wheel  aligned  */ 

852  wheelSelect=  wheelSelect«3;       /*  select  next  servo  (motor)  */ 

853  bitMask  =  bitMask  »  1 ;  /*  select  next  status  align  bit                 */ 

854  } 
855 
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856    *servoControl=OxOOOOOOOO;  /*  disable  all  wheels  */ 

857 

858    /*  clearShaftEncoder(OxOff);  */  /*  clear  all  shaft  encoders  */ 

859 

860  /*  sioOut(0,"aligned ...");    */  /*  Output  Message  */ 

861  return; 

862}  /*  end  of  alignWheels  */ 

863 

864 

865 

866 

867  End  of  servo.c 
868 

869 
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APPENDIX  F:  SOURCE  CODE  (TIMER.C) 


/* * 

2  *  * 

3  *File:  TIMER  .C  * 

4  *  * 

5  *  Environment:  GCC  Compiler  v2.7.2  * 

6  *  Last  update:  29  January  1997  * 

7  *  Name:  Thorsten  Leonardy  * 

8  *  Purpose:       Provides  routines  related  to  the  AM95 13  Timer  Circuit,  such  * 

9  *  as  interrupt  initialization,  * 

10  *  Compiled:      >gcc  -c  -m68040  -o  timer.o  timer.c  * 

11  *  * 

1 2  * *  / 

13 

14 

15  #include  "shepherd.h" 

16  #include  "timer.h" 
17 

18 

19  void  timerStart( void) 

20  { 
21 

"22         long  *vadr; 


23  unsigned  char  *p; 

24  short  *ctrlPort  =  (short*)  TIMER_CTRL; 

25  short  *dataPort  =  (short*)  TIMER_DATA; 
26 

27  /*  initialize  the  interrupt  counter  */ 

28  intCounter=0; 
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29 
30 
31 
32 
33 
34 
35 
36 
37 
38 
39 
40 
41 
42 
43 
44 
45 
46 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 


/*  load  address  for  interrupt  service  routine  */ 

vadr=(long*)VBA_TIMER; 

*vadr=(long)TimerHandler; 

/*  Issue  commands  to  set  control  and  data  register 
/*  refer  to  Fig  1-20,  1-8,  1-12  */ 


*/ 


*ctrlPort=Oxffff; 
*ctrlPort=0xff5f; 
*ctrlPort=Oxffef; 


/*  Master  reset,  clear  data  registers  */ 

/*  load  all  counters  */ 

/*  Set  MM  1 3  (Enter  1 6-bit  bus  mode)    */ 


*ctrlPort=0xffl7;  /*  Select  master  mode  register         */ 

*dataPort=0xale0;  /*  set  master  mode  register  ...        */ 

/*  + >     f  ==  1  sec  interrupt  interval       */ 

/*  e  ==  0. 1  sec  */ 

/*  d==  0.01  sec  */ 

/*  c  =  0.001  sec  */ 

*ctrlPort=0xff05;  /*  Select  CMR  timer  5  ...  */ 

/*  utilize  Data  Pointer  Sequencing     */ 

*dataPort=0x0e32;  /*  and  write  to  counter  mode  register  */ 

/*  + >  f=  10000         */ 

/*  multiply  value  according  to  dataPort  below     e=  1000         */ 
/*  by  the  factor  set  here  to  obtain  timing...     d=    100         */ 
/*  c=     10         */ 

/*  b=      1         */ 
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58  /*  to  obtain  the  correct  timing,  multiply  value  determined  in  data-*/ 

59  /*  port  below  by  the  factor  given  above.  E.g.  dataPort  is  set  to    */ 

60  /*  58  (corresponding  to  lOusec)  and  factor  1000  is  chosen  above,    */ 

61  /*  then  the  interrupt  would  occur  every  10msec!  */ 
62 

63  *dataPort=58;  /*  load  register,  58  ->  lOusec         */ 

64  /*  930  ->  1msec  */ 
65 

66         *ctrlPort=0xff70;  /*  load  and  arm  timer  5  */ 

67 

68  p=(unsigned  char*)ISM_TIMER;  /*  ISM  Configuration  for  Timer         */ 

69  *p=0xcb;  /*  assert  LIRQ-3  to  VIC  */ 
70 

7 1  p=(unsigned  char*)VIC_LIRQ3;  /*  VIC  LICR  for  LIRQ-3  from  ISM        */ 

72  *p=0x03;  /*  assert  IRQ-3  from  VIC  to  68040      */ 
73 

74  return; 

75  }    /*  end  of  timerStart  */ 
76 

77 

79       Assembler  routines 

81 

82     /*  TimerHandler,  its  address  is  set  from  within  timerStart  */ 

83 

84  asm(" 

85  .even 

86  .text 
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87  .globl  _TimerHandler 

88 

89     _TimerHandler: 

90 

91 

92  link      a6,#-184  /*  alocate  184  Bytes  on  stack  to  save  registers      */ 

93  fsave     a6@(-184) 

94  fmovemx  fp0-fp7,sp@-       /*  move  floating  point  registers  80  bit  each         */ 

95  fmovel    fpcr,sp@-  /*  move  floating  point  Control  Regioster  */ 

96  fmovel    fpsr,sp@-  /*  move  floating  point  status  register  */ 

97  fmovel    fpiar,sp@-  /*  move  floating  point  Instruction  address  register  */ 

98  moveml    d0-d7/a0-a5,sp@-    /*  save  data  and  address  registers  (14*4  Byte)       */ 
99 

100 

101  addq.l    #0x0 1  ,_intCounter  /*  increment  interrupt  counter  */ 

102  move.w    #0xffe5,0xfff4 1002/*  clear  toggle  out  for  timer  5  */ 
103 

104  move.l    #0xffff0081,al      /*  load  VME9421  Status  register  */ 

105  eor.b     #0x02,(al)  /*  toggle  green  indicator  light  to  indicate  timer    */ 

106  /*  for  interrupt  handling  is  working  properly  ...    */ 

107  and.b     #0xfe,(al)  /*  turn  red  light  on  to  indicate  that  motion  control*/ 

108  /*  will  start  (this  will  assert  the  SYSFAIL  line  on  */ 

109  /*  the  VME-Bus,  but  we  don't  care  at  this  point).    */ 
110 

111  jsr  _driver  /*  execute  motion  control  part  */ 

112 

113 

1 14  move.l    #0xffff0081,al      /*  load  VME9421  (digital  out  board)  Status  register  */ 

115  or.b      #0x0 1, (a  1)  /*  turn  off  red  indicator  light  to  indicate  that     */ 
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116 

/*  motion  c 

117 

118 

moveml    sp@+,d0-d7/a0-a5 

119 

fmovel    sp@+,fpiar 

120 

fmovel    sp@+,fpsr 

121 

fmovel    sp@+,fpcr 

122 

fmovemx  sp@+,fp0-fp7 

123 

frestorea6@(-184) 

124 

unlk      a6 

125 

126 

rte 

127 

"); 

128 

*/ 


129 

1 3 1      End  of  timer.c 

133 
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APPENDIX  G:  SOURCE  CODE  (MATH.C) 
The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 

1  /* * 

2  *  * 

3  *File:  MATH.C  * 

4  *  * 

5  *  Environment:  GCC  Compiler  v2.7.2  * 

6  *  Last  update:   17  March  1997  * 

7  *  Name:  Thorsten  Leonardy  * 

8  *  Purpose:       A  Simple  Math  library.  * 

9  * */ 

10 

1 1  #include  "shepherd.h" 

12  #include  "math.h" 
13 

1 4  #define  pio4     0.785398 1 63 

1 5  #define  pio2     1 .570796327 
16#definepi       3.141592654 
17#definepi2      6.283185307 
18 


[71 


19 
20 

22  FUNCTION:    norm() 

23  PARAMETERS:  double  angle      —  the  angle  to  normalize 

24  PURPOSE:     normalize  the  input  angle  between  -PI  and  PI 

25  RETURNS:     double:    the  normalized  angle  in  radians 

26  COMMENTS:    This  is  the  most  common  normalizing  function  used  in  the  system 

27  This  performs  that  same  as  norm()  and  normalize)()  in  MML10. 

29  double  norm(double  angle) 

30  { 

3 1  while  ((angle  >  pi)  II  (angle  <=  -pi)) 

32  { 

33  if  (angle  >  pi) 

34  angle  -=  pi2; 

35  else 

36  angle  +=  pi2; 

37  } 

38  return  angle; 

39  } 
40 
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41 
42 
43 

44  /* * 

45  *  * 

46  *  new_abs()  * 

47  *  * 

48  *  Environment:  GCC  Compiler  v2.7.2  * 

49  *  Last  update:   14  March  1997(mod  2  April  97  by  Ed  Mays)        * 

50  *  Name:  Thorsten  Leonardy  * 

5 1  *  Purpose:       A  function  returning  the  absolute  value  of  x.  * 

52  * */ 

53  double  new_abs(double  x) 

54  { 

55  if  (x>=0.0) 

56  return  (x); 

57  else 

58  return(-x); 
59} 

60 
61 
62 
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63 

64  /* * 

65  *  atan2()  * 

66  *  * 

67  *  Environment:  GCC  Compiler  v2.7.2  * 

68  *  Last  update:   17  March  1997  * 

69  *  Name:  Thorsten  Leonardy  * 

70  *  Purpose:       Computes  tan(y/x)  where  x,y  are  real.  If  both  variables  are  * 

71  *  zero,  atan2  returns  zero.  For  any  other  values,  atan2  will  * 

72  *  return  the  positive  angle  for  the  (x,y)-pair,  e.g.,  * 
73*                (x,y)=(0,-l)  would  return  atan2=3/2*pi  !  * 

74  *  ix  determines  the  accuracy  (highest  order  term  in  expansion)' 

75  *  For  the  worst  case,  ly/xl  close  to  one,  ix  should  be  very 

76  *  high.  Here  is  some  data:  * 

77  *  Ix/yl       ix  accuracy  of  result  [rad]  * 

78  *       0.9   101    +-  1.88*10E-7  * 

79  *  1001   +-  1.57*10E-49  * 

80  *       0.99   101    +-3.45*10E-3  * 

81  *  1001   +-4.18*10E-8  * 

82  *  10001   +-2.18*10E-48  * 

83  *       0.999   101    +-8.76*10E-3  * 

84  *  1001   +-3.65*10E-4  * 
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\* 


* 


85  *  10001       +-4.50*10E-9  * 

86  * */ 

87  double  atan2(double  y,  double  x) 

88  { 

89  double  erg=0.0,  z=0.0,  z2; 

90  int  ix=  1 0 1 ,  flag  1  =0,flag2=0; 
91 

92  if  ((new_abs(y)>new_abs(x))&&(y!=0)) 

93  { 

94  z=x/y;  /*  in  case  ly/xl>l  compute  atan(l/z)  */ 

95  flagl=(y>0)-(y<0);      /*  a  handy  sign-function  */ 

96  } 

97  elseif(x!=0) 

98  { 

99  z=y/x;  /*  in  case  ly/xkl  compute  atan(z)  */ 

100  flag2=(x<0.0);  /*  in  this  case  need  to  add  pi  to  final  result  */ 

101  } 
102 

103  /*  From  here  on  Izl  must  always  be  less  than  one  !!!  */ 

104  z2=z*z; 
105 

106     /*  Taylor  expansion  */ 
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107  if  (new_abs(z)<1.0)  {  I*  computation  for  ly/xkl  */ 

108  while  (ix>l){ 

109  erg=z2*(  1 .0/ix-erg);  /*  try  alternatively  for  accuracy:  (z2/ix)*(  1 .0-ix*erg)  */ 

1 10  ix-=2; 


111 


} 


112  erg=z-z*erg; 

113  } 

1 14  else  erg=((z>0.0)-(z<0.0))*pio4;    /*  for  ly/xl=l  result  is  either  +-  pi/4  */ 
115 

116  if  (flag  1  ==  1 )  erg=pio2-erg;        /*  point  lies  in  3rd  or  4th  octant  for  flag  1  =+ 1  */ 

1 17  else  if  (flagl==-l)  erg=-pio2-erg;  /*  ...  or  in  6th  or  7th  octant  for  flagl=-l      */ 

118  if  (flag2)  erg=erg+pi;  /*  point  lies  in  4th  or  5th  octant  */ 

119  /*  if  (erg<0.0)  erg=erg+pi2;        deleted  6/27/97  */ 
120 

121      return(erg); 

122} 

123 

124 

125/* * 


126*atan()     yk 
127  * 


-*/ 


128double  atan(double  x) 
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129{ 

1 30  double  erg=0.0,  z=0.0,  z2; 

131  intix=101,flag=0; 

132 

133  if(x==0.0) 

134  return  (0.0); 

135  if  (ne w_abs(new_abs(x)- 1 .0)  <  0.0000 1 ) 

136  return  (pio4  *  x);       /*  return  +-  pi/4  */ 

137  if  (new_abs(x)  >  1.0) 

138  { 

139  z=1.0/x;  /*  in  case  lxl>l  compute  atan(l/x)  */ 

140  flag=(x>0)-(x<0);      /*  a  handy  sign-function  */ 

141  } 

142  else 

143  z=x;  /*  in  case  Ixkl  compute  atan(x)  */ 

144  z2=z*z;  /*  From  here  on  Izl  is  less  than  one  !!!  */ 

145  /*  Taylor  expansion  */ 

146  while  (ix>l) 

147  { 

148  erg=z2*(1.0/ix-erg); 

149  ix-=2; 

150  } 

177 


151  erg=z-z*erg; 

152  if  (flag  ==  1)  erg  =  pio2-erg; 

1 53  if  (flag  ==  - 1 )  erg  =-pio2-erg; 

154  return(erg); 
155} 

156 
157 
158 
1 59/* * 

160  *  cos()  * 

161  *  * 

162  *  Environment:  GCC  Compiler  v2.7.2  * 

163  *  Last  update:   17  March  1997  * 

164  *  Name:  Thorsten  Leonardy  * 

165  *  Purpose:       Computes  cos(x)  where  x  can  be  any  real  number.  * 

166  *  ix  determines  the  accuracy  (highest  order  term  in  expansion)* 

167  * */ 

168double  cos(double  x) 

169{ 

170  double  erg; 

171  int  quadrant,  ix=20;    /*  ix  must  be  an  even  number         */ 
172 

178 


173  /*  analyze  and  reduce  x  to  the  appropriate  range  ...  */ 

174  quadrant=(x/pio2+(x>=0)-(x<0))/2;  /*  determine  in  what  sector  x  is     */ 

175  x=x-quadrant*pi;  /*  reduce  x  to  region  [-pi/2... pi/2]*/ 

176  x=x*x;  /*  compute  xA2  and  store  in  x        */ 

177  erg=1.0; 
178 

179  /*  the  cosine  taylor  computation  is  a  one-liner  ;-)  */ 

180  while  (ix>0)  { 

181  erg=1.0-erg*x/ix/(ix-l); 

182  ix-=2; 

183  } 
184 

185  /*  shift  sign  if  quadrant  is  not  1,3,5,...  */ 

1 86  if  (quadrant%2)  erg=-erg; 
187 

188    return(erg); 

189} 

190 

191 

1 92/* * 

193  *  sin()  * 

194*  * 
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195  *  Environment:  GCC  Compiler  v2.7.2  * 

196  *  Last  update:   14  March  1997  * 

1 97  *  Name:  Thorsten  Leonardy  * 

198  *  Purpose:       Computes  sin(x)  where  x  can  be  any  real  number.    * 

199  * */ 

200double  sin(double  x) 

201  { 

202  return(cos(x-pio2));  /*  since  sin(x)=cos(x-pi/2)  */ 
203} 

204 
205 
206 
207/* * 

208  *  sqrt()  * 

209  *     Ed  Mays  and  Ferdinand  Reid  March  1997  * 

210  *  Environment:  GCC  Compiler  v2.7.2  * 

21 1  * —  */ 

212double  new_sqrt(double  x) 

213  { 

214  double  xl,x2; 

215  int  count; 
216 
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217 

218  if (x  ==  l.O)return(l.O); 

219  xl  =  1.0; 

220  for  (count=0;  count  <  10;  count++){ 

221  x2  =  .5  *(xl  +x/xl); 

222  xl=x2; 

223  } 
224 

225  return  (x2); 

226  } 
227 
228 
229 
230 

23 1/* 

232  *  new_sqrtl() 

233  *      Ed  Mays  and  Kanayama 

234  *  Environment:  GCC  Compiler  v2.7.2 

235  *  ----- 

236double  new_sqrtl  (double  x) 

237  { 

238  double  xl,x2; 


18 


239  xl  =  1.0; 

240  x2=-1.0; 

24 1  while(new_abs(x  1  -x2)  <  1 .0e-9) 

242  { 

243  x2  =  xl; 

244  xl  =  .5*(x2  +  x/x2); 

245  } 
246 

247  return  (xl); 

248  } 
249 
250 
251 
252 

253/*  ed  move  to  math.c*/ 
254double  min  (double  a,  double  b) 

255  { 

256  if(a<=b) 

257  return  a; 

258  else 

259  return  b; 
260} 
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261 

262/*  ed  move  to  math.c*/ 

263double  max  (double  a,  double  b) 

264  { 

265  if(a>=b) 

266  return  a; 

267  else 

268  return  b; 
269} 

270/*ed*/ 
271 

272 
273 
274 
275 
276 
277 
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278  End  of  math.c 
279 
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APPENDIX  H:  SOURCE  CODE  (UTILS.C) 

1  /* * 

2  *  * 

3  *FILE:  UTILS  .C  * 

4  *  * 

5  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

6  *  LAST  UPDATE:  03  FEBRUARY  1997  * 

7  *NAME:  THORSTEN  LEONARD Y  * 

8  *  PURPOSE:       PROVIDES  THE  UTILITY  FUNCTIONS  FOR 

PROGRAM  SHEPHERD.  * 

9  *  * 

10  *  COMPILED:      >GCC -C -M68040 -OUTILS.O  UTILS.C  * 

1 1  *  * 

12  * */ 

13 

14  #INCLUDE  "SHEPHERD.H" 

15  #INCLUDE  "UTILS.H" 

16  #INCLUDE  "MATH.H" 
17 

18 

1 9  UNSIGNED  INT  PIFLAG=0; 

20  UNSIGNED  INT  MAGIC=0X  1 237; 
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2 1  EXTERN  CHAR  JOYSTICK^] ;     /*  DEFINED  IN  SHEPHERD.C  */ 

22  EXTERN  CHAR  BCDSTRING[];  /*  DEFINED  IN  SHEPHERD.C  */ 
23 

24 

25  /* * 

26  *  READCLOCKO  * 

27  *  * 

28  *         ENVIRONMENT:  GCC         COMPILER        V2.7.2 
* 

29  *  LAST  UPDATE:  26  FEBRUARY  1997  * 

30  *NAME:  THORSTEN  LEONARD Y  * 

3 1  *  PURPOSE:       THIS  FUNCTION  READS  THE  VALUES  FROM  THE 
CALENDAR  CLOCK       * 

32  *  DEVICE  MTK48T08  (SEE  OMNIBYTE  HANDOUT  CHAP. 
2.9.4)  INTO      * 

33  *  GLOBAL  VARIABLE  CLOCK.  THE  FORMAT  IN  CLOCK 
IN  DECIMAL  IS:    * 

34  *  * 

35  *  CLOCK  =  YYMMDDHHMMSS  * 

36  *  * 

37  *  I.E  TO  RETRIEVE  THE  DATE  PERFORM 
DATE=CLOCK/ 1000000;         * 

38  *  TO  RETRIEVE  THE  TIME  PERFORM 
TIME=CLOCK%  1000000;         * 

39  *  * 

40  *  CALLED  BY:         FUNCTION  TIMERHANDLER  IN  TIMER.C 
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41  * 

42  * 

43 
44 
45 
46 
47 

48  /* 

49  *  PITEST() 

50  * 

51  *         ENVIRONMENT: 


*/ 


GCC         COMPILER         V2.7.2 


52  *  LAST  UPDATE:  24  FEBRUARY  1997  * 

53  *NAME:  THORSTEN  LEONARD Y  * 

54  *  PURPOSE:  THIS  FUNCTION  TESTS  INTERPROCESS  OR 
SIGANNLING  VIA  PI-46      * 

55  *  INTERRUPT.  * 

56  * */ 

57  VOID  PITEST(VOID) 

58  { 

59  LONG  *VADR; 

60  UNSIGNED  CHAR  *P; 

61 

62  /*  SET  ADDRESS  FOR  PROCESSOR  INTERRUPT  HANDLER 

ROUTINE  */ 
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63  VADR=(UNSIGNED  LONG  *)VBA_PI; 

64  *VADR=(UNSIGNED  LONG)PfflANDLER; 
65 

66  P=(UNSIGNED  CHAR  *)ISM_PI; 

67  *P=(UNSIGNED  CHAR)0XE0;  /*  SET  IP-ISM  TO  68040  ON 
LIRQ-6  */ 

68 

69  P=(UNSIGNED  CHAR  *)VIC_LIRQ6; 

70  *P=(UNSIGNED  CHAR)0X06;  /*  CONFIGURE  VIC068  LIRQ-6 

*/ 

71 

72  P=(UNSIGNED   CHAR    *)APP_ICR;  /*    ABORT/PROC 
INTERRUPT  CTRL  */ 

73  *P  =  *P  I  (UNSIGNED  CHAR)0X02;  /*  ASSERT  IP-46  INTERRUPT 

*/ 

74 

75  WHILE(PIFLAG==0)   {   INTCOUNTER=0;    }  /*  WAIT  FOR  PI 

INTERRUPT  */ 

76 

77  IF  (PIFLAG==MAGIC)  { 

78  /*  TOGGLEVME((UNSIGNED  CHAR  *)VME92 10,0X02);  */ 

79  SIOOUT(0,"PASSED"); 

80  } 

81  ELSE  { 

82  /*    TOGGLEVME((UNSIGNED  CHAR  *)VME942 1,0X02);*/ 

83  SIOOUT(0,"FAILEDM); 
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84         } 
85 

86  RETURN; 

87  }    /*  END  OF  PITEST  */ 
88 

89 
90 

91  /* 

92  *  SETVMEO 

93  * 

94  *         ENVIRONMENT: 


GCC         COMPILER         V2.7.2 


95  *  LAST  UPDATE:  24  FEBRUARY  1997  * 

96  *NAME:  THORSTEN  LEONARD Y  * 

97  *  PURPOSE:  THIS  FUNCTION  OUTPUTS  DATA  TO  THE 
STATUS  REGISTER  OF  THE     * 


98  * 

99  * 


SPECIFIED  VME  BOARD. 


7 


100  VOID  SETVME(UNSIGNED  CHAR  *BOARDADDRESS,  UNSIGNED 
CHAR  DATA) 

101  { 

102  BOARDADDRESS  =  BOARDADDRESS  +  0X81;        /*  ACCESS 
STATUS  REGISTER  */ 


103  *BOARDADDRESS=DATA; 

104  RETURN; 

105  } 
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/*  WRITE  DATA 


•7 


106 
107 
108 

109  /* 

110  *  TOGGLE VME() 

111  * 

112  *         ENVIRONMENT: 


GCC        COMPILER         V2.7.2 


113  *  LAST  UPDATE:  24  FEBRUARY  1 997 

114  *NAME:  THORSTEN  LEONARD Y 


115       *    PURPOSE: 
OPERATION  ON  THE  STATUS 


THIS    FUNCTION    PERFORMS    AN    XOR 

* 


116 


REGISTER  OF  THE  SPECIFIED  VME  BOARD. 


117 


7 


118  VOID    TOGGLEVME(UNSIGNED    CHAR    *BOARD,    UNSIGNED 
CHAR  DATA) 

119  { 

1 20  BOARD  =  BOARD  +  0X8 1 ;     /*  ACCESS  STATUS  REGISTER  */ 

121  *BOARD  =  *BOARD  A  DATA;   /*  TOGGLE  BIT  WITH  BITWISE 
XOR*/ 

122  RETURN; 

123  } 
124 
125 
126 
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127  /* * 

128  *  INITBOARDSO  * 

129  *  * 

130  *  ENVIRONMENT:  GCC  COMPILER  V2. 7. 2  * 

131  *  LAST  UPDATE:  24  FEBRUARY  1997  * 

132  *NAME:  THORSTEN  LEONARD Y  * 

133  *  PURPOSE:       THIS  FUNCTION  INITIALIZES  ALL  VME  BOARDS. 

134  * */ 

135  VOID  DMITBOARDS(VOID) 

136  { 

1 37  UNSIGNED  CHAR  *P; 

138  INT  DC; 
139 

140  P=(UNSIGNED  CHAR*)VIC_TTR;  /*  VIC  TRANSFER 
TIMEOUT  REGISTER       */ 

141  *P  =  0XFF;  /*  DISBLE  ALL  WATCHDOGS  */ 

142 

143  P=(UNSIGNED  CHAR*)VIC_ICR;  /*  VIC  INTERFACE 
CONFIGURATION  REG.    */ 

144  *P=0X40;  /*  PREVENT  DEADLOCKS,  THIS  IS  A 
MUST!  */ 

145 

146  P=(UNSIGNED  CHAR* )VME942 1+0X81;    /*  ACCESS  STATUS 

REGISTER  FOR  DI       */ 
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147         *P  =  0X03;  /*  DISABLE  SYSFAIL  SIGNAL,  SET 

GREEN  */ 

148 

149  P=(UNSIGNED  CHAR*)VME92 10+0X81;     /*  ACCESS  STATUS 
REGISTER  FOR  DA       */ 

150  *P  =  0X03;  /*  DISABLE  SYSFAIL  SIGNAL,  SET 
GREEN  */ 

151 

152 

153       /* * 


154  *  INITIALIZE  ALL  EIGHT  QUADRATURE  COUNTERS  (WHEEL 
ENCODER)  * 

155  * */ 

156 

1 57  P=(UNSIGNED  CHAR*)VMECTR  1 ; 

158  FOR  (DC=0;  IX<8;  DC++)  {  /*  READ  MOTORS 
SUBSEQUENTIALLY    */ 

/*  CR:  MASTER  RESET  */ 

/*  IC:  ENABLE  COUNTING  */ 

/*  QR:  COUNT  FULL  CYCLE  */ 

/*  ACCESS  NEXT  COUNTER  */ 

163  IF  (K=3)  P=(UNSIGNED  CHAR*)VMECTR2;  /*  ACCESS  THE 
SECOND  VME  COUNTER  */ 

164  } 
165 

1 66       SIOOUT(0,"BOARDS  INITIALIZED  ...\N\R"); 
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159 

*(P+3)=0X20; 

160 

*(P+3)=0X48; 

161 

*(P+3)=0XC1; 

162 

P=P+0X04; 

167 

168  RETURN; 

169  } 
170 

171  /*  MODIFIED  ED  MAYS  1 8  APR  97  */ 

1 72  UNSIGNED  CHAR  B2A(INT  VALUE) 

173  { 

1 74  UNSIGNED  CHAR  CHAR; 
175 

176  IF  (VALUE  <  10)  { 

1 77  CHAR  =  48  +  VALUE; 

178  } 

179  ELSE  { 

1 80  CHAR  =  55  +  VALUE; 

181  } 

182  RETURN  CHAR; 

183  } 
184 

1 85  VOID  B2A2(UNSIGNED  CHAR  *S,  UNSIGNED  CHAR  CC) 

186  { 

187  INT  LOW,  HIGH; 
188 

189  LOW  =  CC  &  0X0F; 

190  HIGH  =  CC/16; 

193 


191 

192  *S  =  B2A(L0W); 

193  *(S-1)  =  B2A(HIGH); 

194  } 
195 
196 
197 


198      ASSEMBLER  ROUTINES 
199 


200 

201  ASM(" 

202  .EVEN 

203  .TEXT 

204  .GLOBL  _PIHANDLER 
205 

206  _PIHANDLER: 

207  MOVE.L  #0XFFF4800C,A: 


*/ 


208  AND.B    #0XFD,(A1) 

INTERRUPT  SIGNAL  */ 


/*  LOAD  APP-ICR  INTO  Al 


/*  REMOVE  PENDING  IP-46 


209 


MOVE.L  #OX1237,_PIFLAG         /*  SET  PIFLAG  VARIABLE 


210  RTE 
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211  "); 

212 

213 

214  /* * 

215  *  CONVERTTOASCHO  * 

216  *  * 

217  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

218  *  LAST  UPDATE:  02  MAY  1 997  * 

219  *NAME:  THORSTEN  LEONARD Y  * 

220  *  PURPOSE:       THIS  FUNCTION  CONVERTS  AN  UNSIGNED 
INTEGER  TO  ITS  ASCII      * 

221  *  EQUIVALENT  AND  WRITES  THIS  INTO  A  STRING. 

222  *    NDIGITS  NUMBER    OF    DIGITS    TO    CONVERT 

223  *DATA  THE  INTEGER  TO  CONVERT  * 

224  *STR  POINTER  TO  STRING  * 

225  * */ 

226  VOID  CONVERTTOASCn(UNSIGNED  INT  NDIGITS,  UNSIGNED 
INT  DATA,  CHAR  *STR) 

227  { 

228  UNSIGNED  INT  I; 
229 

230  STR=STR+NDIGITS-1; 

231  FOR  (I=0;I<NDIGITS;I++)  { 
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* 


232  *STR--  =  '0'  +  DATA  %10  ; 

233  DATA=DATA/10; 

234  } 

235  RETURN; 

236  } 
237 
238 

239  /* 

240  *  READJOYSTICK() 

241  * 

242  *         ENVIRONMENT: 


GCC         COMPILER         V2.7.2 


243  *  LAST  UPDATE:  02  MAY  1997  * 

244  *NAME:  THORSTEN  LEONARD Y  * 

245  *  PURPOSE:        THIS  FUNCTION  READS  THE  THREE  PORTS  (A,B 
AND  C)  FROM  THE     * 

246  *  INTEL  85C55  PARALLEL  PORT  1  AND  CONVERTS 
THEM  INTO  AN  ASCII  * 

247  *  STRING.  * 

248  *  — */ 

249 

250  VOID  READJOYSTICK(VOID) 

251  { 

252  UNSIGNED  INT  I,INDEX; 

253  UNSIGNED  CHAR  *CTRLPORT=(UNSIGNED  CHAR*)PI01_CTRL; 
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254  UNSIGNED  CHAR  *DATAPORT=(UNSIGNED 
CHAR*)PI01_DATA; 

255  UNSIGNED  INT  PIOPORT 1  [3] ; 

256  DOUBLE    A=  0. 1 ,  XX,  YY,  ZZ; 

257 

258  *CTRLPORT=0X9B;      /*  SET  ALL  PORTS  (A,B,Q  INTO  INPUT 
MODE  (READ  ONLY)  */ 

259  INDEX=10;  /*  POSITION  FOR  X-DIGITS  IN  STRING 
JOYSTICK           */ 

260 

261  FOR  (I=0;I<3;I++) 

262  PIOPORT  1  [I]  =  *(DATAPORT+I); 
263 

264  XX  =  (DOUBLE)PIOPORT1[0]- 128.0; 

265  YY  =  (DOUBLE)PIOPORTl[l  ]- 128.0; 

266  IF  (XX  >=  0.0) 

267  XX  =  XX*XX/100; 

268  ELSE 

269  XX  =  -XX*XX/100; 

270  IF  (YY  >=  0.0) 

271  YY=  YY*YY/100; 

272  ELSE 

273  YY  =  -YY*YY/100; 

274  JOYSTICK.X  =  A*(XX)  +  ( 1  0-A)*JOYSTICK.X; 

275  JOYSTICK.  Y  =  A*(  YY)  +  ( 1 .0-A)*JOYSTICK.  Y; 
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276 

277  IF(PIOPORT1[2]==OX03) 

278  SETVME((UNSIGNED  CHAR  *)VME92 10,0X00);  /*  NO  BUTTON 
PRESSED      */ 

279  ELSE  { 

280  SETVME((UNSIGNED  CHAR  *)VME92 10,0X02);     /*  IF  ANY 
BUTTON  PRESSED  */ 

281  } 

282  } 
283 
284 

285  DOUBLE  INSENSITIVE(DOUBLE  Z) 

286  { 

287  IF(Z>=10.0) 

288  RETURN  (Z  -  10.0); 

289  ELSE 

290  IF(Z<=-10.0) 

291  RETURN  (Z  +  10.0); 

292  ELSE 

293  RETURN  0.0; 

294  } 
295 

296  VOID  DISPLAYJOYSTICK() 

297  { 

298  CONVERTINT(BCDSTRING+9,  (INT)JOYSTICK.X); 
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299  BCDSTRING[3]=,0,; 

300  BCDSTRING^T; 

301  BCDSTRING^]^'; 

302  BCDSTRING[7]=,0'; 

303  SIOOUT(0,BCDSTRING); 
304 

305  CONVERTINT(BCDSTRING+9,  (INT)JOYSTICK.OMEGA); 

306  BCDSTRINGPKO*; 

307  BCDSTRING[4]=,4, 

308  BCDSTRING[6]=,4, 

309  BCDSTRINGm^O" 

310  SIOOUT(0,BCDSTRING);        /*   OUTPUT  UPDATED  POSITION 
STRING  TO  SCREEN  */ 

311  RETURN; 

312  } 
313 

314 

315  ENDOFUTILS.C 
316 


199 


200 


APPENDIX  I:  SOURCE  CODE  (SERIAL.C) 

1  /* * 

2  *  * 

3  *FILE:  SERIAL.C  * 

4  *  * 

5  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

6  *  LAST  UPDATE:  26  FEBRUARY  1997  * 

7  *NAME:  THORSTEN  LEONARD Y  * 

8  *  PURPOSE:       PROVIDES  ROUTINES  FOR  SERIAL  INPUT 
AND  OUTPUT  TO  THE  68C681  * 

9  *  ON  THE  TAURUS  BOARD.  * 

10  *  * 

1 1  *  COMPILED:      >GCC  -C  -M68040  -O  SERIAL.O  SERIAL.C* 

12  *  * 

1 3  *  — *  / 

14 

15  #INCLUDE  "SHEPHERD.H" 

16  #INCLUDE  "SERIAL.H" 
17 

18  /* * 

19  *  GLOBAL  VARIABLES  * 

20  *  - */ 

21 
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22  /*  UNSIGNED  INT  COUNTER;        /*  COUNT  THE  INTERRUPTS  */ 

23  UNSIGNED  CHAR    INPORTA;  /*  CHARACTER  READ  FROM  SERIAL 
PORT  */ 

24 

25     /*  VT1 00  CONTROL  SEQUENCES  */ 

26 

27  /*  POSITION  CURSOR,  CUP  =  ESC  [  '0'  '0' ;  '0'  '0'  H  */ 

28  UNSIGNED   CHAR  VT100XY[9]=  {27,91,48,48,59,48,48,72,0};      /*   POSITION 
CURSOR  */ 

29 

30  /*  ERASE  IN  DISPLAY  ED  TO  CLEAR  THE  SCREEN  */ 

31  UNSIGNED  CHAR  CLRSCR[5]=  {27,91,50,74,0};        /*  ESC  [  '2'  J  */ 
32 

33  /*  ESC-SEQUENCE  EL  (ERASE  IN  LINE)  TO  ERASE  A  LINE  */ 

34  UNSIGNED  CHAR  CLRLINE[6]=  {5,27,91,50,75,0};    /*  ESC  [  *2'  K  */ 
35 

36  /*  ESC-SEQUENCE  PRINT  SCREEN  (ESC  [  I)  */ 

37  UNSIGNED  CHAR  PRTSCR[4]=  {27,91,105,0};         /*  ESC  [  I  */ 
38 

39 

40  /*  ESC-SEQUENCE  SGR  (SELECT  GRAFIK  RENDITION)  (ESC  [  0  M )  */ 

41  UNSIGNED  CHAR  CURSOROFF[5]  =    {27,91,0,109,0};       /*  CURSOR  BLINK 
OFF*/ 

42 

43      /* ----- - —  * 


202 


44  *  SIOOUTO  * 

45  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

46  *  LAST  UPDATE:  07  JANUARY  1997  * 

47  *NAME:  THORSTEN  LEONARD Y  * 

48  *  PURPOSE:       THIS  FUNCTION  OUTPUTS  A  STRING 
TO  ONE  OF  THE  TWO  SERIAL  * 

49  *  PORTS.  * 

50  *  * 

51  *  HOSTFLAG       0  ->  OUTPUTS  TO  CONSOLE  (PORT  A)  * 

52  *  1  ->  OUTPUTS  TO  HOST     (PORT  B)  * 

53  *  * 

54  *S  POINTER  TO  THE  OUTPUT  STRING  * 

55  *  * 

56  * */ 

57 

58  VOID  SIOOUT(INT  HOSTFLAG,  UNSIGNED  CHAR  *S) 

59  { 

60  UNSIGNED  CHAR  *P=(UNSIGNED  CHAR  *)CONSOLE; 
61 

62  IF  (HOSTFLAG)  P+ =8;  /*  ACCESS  HOST  REGISTERS     */ 

63  /*  OTHERWISE  ACCESS  CONSOLE  */ 

64  WHILE(*S)  { 

65  WHILE  ((*(P+1)&4)==0);    /*  SRA:  WAIT  UNTIL  TX  READY  */ 

66  *(P+3)=*S+  +  ;  /*  OUTPUT  CHARACTER  */ 
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67         } 
68 

69  RETURN; 

70  }   /*  END  OF  SIOOUT  */ 

71 
72 

73 

74  /* * 

75  *  GOTOXYO  * 

76  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

77  *  LAST  UPDATE:   14  FEBRUARY  1997  * 

78  *NAME:  THORSTEN  LEONARD Y  * 

79  *  PURPOSE:  THIS  FUNCTION  POSITIONS  THE  CURSOR  ON  THE 
SCREEN.            * 

80  *  * 

81  *  X  ROW  FOR  CURSOR  POSITION  (X=0..20)  * 

82  *Y  COLUMN  FOR  CURSOR  POSITION  (Y=l.. 80)  * 

83  *  * 

84  * */ 

85 

86  VOID  GOTOXY(INT  X,  INT  Y) 

87  { 
88 

89  IF  ((X>0)&(X<81)&(Y>0)&(Y<33))  { 
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90  VT100XY[2]=48+X/10; 

91  VT100XY[3]=48+X%10; 

92  VT100XY[5]=48+Y/10; 

93  VT100XY[6]=48+Y%10; 

94  SIOOUT(0,VT100XY);     /*  OUTPUT  ESCAPE  -SEQUENCE  */ 

95  } 

96  RETURN; 

97  } 
98 

99 

100  /*  - * 

101  *  SIOINITO  * 

102  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

103  *  LAST  UPDATE:  26  FEBRUARY  1997  * 

104  *NAME:  THORSTEN  LEONARD Y  * 

105  *  PURPOSE:       THIS  FUNCTION  INITIALIZES  BOTH 

SERIAL  PORTS.  IN  ADDITION,  * 

106  *  PORT  A  (CONSLE)IS  INITIALIZED  FOR 

INTERRUPT  DRIVEN  I/O  * 

1 07  * *  / 

108 

109  VOID  SIOINIT(VOID) 

110  { 
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1 1 1  UNSIGNED  CHAR  *P=  (UNSIGNED  CHAR*)CONSOLE;/*  BASE  ADDRESS 
FOR  68C681  DUART  */ 

112  LONG*VADR;  /*  FOR  VBA  REGISTER  ENTRY         */ 
113 

114 

115  /* *  / 

116  /*  INITIALIZE  CONSOLE  (PORT  A)  */ 

117  /* *  / 

118  /*  ATTENTION:  THESE  SETTINGS  HAVE  TO  AGREE  WITH  THE 
SETTINGS  FOR         */ 

119  /*  YOUR  TERMINAL  (I.E.  LAPTOP  COMPUTER)  */ 

120  *(P+2)=(UNSIGNED  CHAR)0X2A;         /*  CRA:  RESET  RXJDISABLE  RX  & 
TX       */ 

121  *(P+2)=(UNSIGNED  CHAR)0X1A;  /*  CRA:  RESET  MR  POINTER, 

*/ 

122  *(P+0)= (UNSIGNED  CHAR)0X13;  /*  MR1A:  RX  CONTROLS  RTS, 

V 

123  /*        8  BITS,  NO  PARITY  */ 

124  *(P+0)= (UNSIGNED  CHAR)0X07;         /*  MR2A:  NORMAL  MODE,  1  STOP 
BIT        */ 

125  *(P+1)=(UNSIGNED  CHAR)0XBB;  /*  SET  BAUD  RATE  9600  BAUD 

*/ 

126  *(P+2)= (UNSIGNED    CHAR)0X15;  /*    ENABLE    RX   AND   TX 

V 

127 
128 

1 29  /*  — - - *  / 

1 30  /*  INITIALIZE  HOST  (PORT  B)  */ 
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131  /* *  / 

132  *(P+ 10)= (UNSIGNED  CHAR)0X1A;  /*  CRB:  RESET  MR  POINTER 

V 

133  *(P+ 8)= (UNSIGNED  CHAR)0X13;  /*  MR1B:  NO  PARITY,  8  BITS 

*/ 

134  *(P+ 8)= (UNSIGNED  CHAR)0X07;         /*  MR2B:  NORMAL  MODE,  1  STOP 
BIT        */ 

135  *(P+9)=(UNSIGNED  CHAR)0XBB;  /*  SET  BAUD  RATE  9600  BAUD 

*/ 

136  *(P+ 10)= (UNSIGNED  CHAR)0X15;  /*  CRB:  ENABLE  RX  AND  TX 

*/ 

137 
138 

1 39  /* *  / 

140  /*   IT  FOLLOWS  THE  INTERRUPT  SPECIFIC   PART  FOR  PORT  A 

*/ 

141  /* *  / 

142  *(P+ 5)= (UNSIGNED  CHAR)0X02;        /*  ISR:  SET  INTERRUPT  MASK  FOR 
RXRDYA*/ 

143  *(P+12)=0X60;  /*  WR:  PLACE  INTERRUPT  VECTOR  */ 

1 44  /*  0X60  ACCESSES  VBA  AT  BASE+0X1 80      */ 

145  VADR=(LONG*)0XFFE40180;  /*  VBA  ADDRESS  FOR  INTHANDLER 

*/ 

146  *VADR=(LONG)INPORTAHANDLER;        /*  WRITE  ADDRESS  INTO  VBR 
V 

147 

148  P= (UNSIGNED   CHAR*)ISM_SERIAL;  /*    ISM    CONFIGURATION 
REGISTER  FOR  SIO  */ 

149  *P=0X09;  /*  INTERRUPTS  TO  68040  ON  LIRQ-1         */ 
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150 

151  P=(UNSIGNED  CHAR*)VIC_LIRQ1 ;       /*  VIC068  LICR  FOR  LIRQ-1  FROM 

ISM      */ 

1 52  *P=0X01 ;  /*  ASSERT  IRQ-1  FROM  VIC  TO  68040       */ 
153 

154 

155  RETURN; 

156  } 
157 
158 
159 
160 

162  ASSEMBLER  ROUTINES 

163  ********************************************************** 

164 

165  /* * 

166  *  INPORTAHANDLERO  * 

167  *  * 

168  *  ENVIRONMENT:  GCC  COMPILER  V2.7.2  * 

169  *  LAST  UPDATE:  27  JANUARY  1997  * 

170  *NAME:  THORSTEN  LEONARDY  * 

171  *  PURPOSE:  INTERRUPT  HANDLING  ROUTINE  FOR  INTERRUPTS 
FROM  68C681  DUART  * 
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1 72     *  IT  READS  A  CHARACTER  INPUT  FROM  THE  KEYBOARD  INTO 

VARIABLE  * 


173      * 
CHARACTER     * 


INPORTA,  INCREMENTS  A  COUNTER,  AND  OUTPUTS  THE 
TO  THE  SCREEN.  IF  A  CR  IS  TYPED  AT  THE  KEYBOARD,  AN 


175  *  ADDITIONAL  LINEFEED  (OXOA)  IS  ADDED  TO  THE  <CR> 

(OXOD).      * 

176  *  * 

1 77  * *  / 

178 

179    ASM(" 

180 

181  EVEN 

182  .TEXT 

1 83  .GLOBL  _INPORTAHANDLER 
184 

185  _INPORTAHANDLER: 
186 

187  LINK      A6,#-128  /*  ALLOCATE  184  BYTES  ON  STACK  TO  ...  */ 

188  FSAVE     A6@(-128) 

189  MOVEML      D0-D7/A0-A5,SP@-     /*  SAVE  REGISTERS  (14*4  BYTE) 


190 
191 


MOVE.L  #0XFFF4A000,A2         /*  BASE  ADDRESS  OF  68C681  DUART 


7 


192  MOVE.B  3(A2),D2  /*  RHR_A:  READ  CHARACTER 
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193  MOVE.B  D2,_INPORTA  /*  ...  AND  COPY  TO  INPORTA  */ 

194 

195  MOVEML    SP@+,D0-D7/A0-A5 

196  FRESTOREA6@(-128) 

197  UNLK      A6 
198 

199 

200  RTE 

201  "); 
202 
203 
204 

205  /************************************************* 

206  END  OF  SERIAL.C 
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APPENDIX  J:  SOURCE  CODE  (CONSOLIDATED  HEADER  FILES) 

The  following  code  was  modified  by:  Professor  Kanayama,  Thorsten  Leonardy, 
Edward  Mays,  and  Ferdinand  A.  Reid. 

1  /*  shepherd.h  */ 

2 

3  #ifndef  SHEPHERD.H 

4  #define  SHEPHERD.H 
5 

6  /* * 

7  *  Base  Addressees  for  accessing  Servo  Control  Cards  * 

8  * */ 

9 

10#defineVME9210  0xffff0400     /*  Base  Address  analog  out  to  servo      */ 

1 1  #define  VME9421  OxffffOOOO     /*  Base  address  data  in  from  servo       */ 

12#defineVME2170  OxffffffOO     /*  Base  address  data  out  to  servo        */ 

1 3  #define  VMECTR 1     0xffff6000     /*  VME  Counter  for  driving  motor        */ 

14#define  VMECTR2     0xffff6100     /*  VME  Counter  for  steering  motor        */ 

15 

16 

17  /* * 

18  *  defines  for  general  Interrupt  Handling  * 
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19  * 

20 
21#defineVIC 

22  #define  VIC 

23  #define  VIC 

24  #define  VIC. 

25  #define  VIC. 

26  #define  VIC. 

27  #define  VIC. 
28 


LIRQ1  0xfff44027 
LIRQ2  0xfff4402b 
LIRQ3  0xfff4402f 
LIRQ4  0xfff44023 
LIRQ5  0xfff44037 
LIRQ6  0xfff4403b 
LIRQ7  0xfff4403f 


/*  VIC068  Register  for  LIRQ-1  */ 
/*  VIC068  Register  for  LIRQ-2  */ 
/*  VIC068  Register  for  LIRQ-3  */ 
/*  VIC068  Register  for  LIRQ-4  */ 
/*  VIC068  Register  for  LIRQ-5  */ 
/*  VIC068  Register  for  LIRQ-6  */ 
/*  VIC068  Register  for  LIRQ-7  */ 


29  #define  VIC_TTR     0xfff44043  /*  Transfer  Timeout  Register  */ 

30  /*  see  p.  4-2  TAURUS  Manual  */ 

3 1  #define  VIC_ICR     0xfff440af  /*  VIC  Interface  Configuration  */ 
32 

33 

34  #define  enable()    asm("move.w  #0x2000,sr")  /*  enable  interrupts  */ 

35  #define  disable()  asm("move.w  #0x2700,sr")  /*  disable  interrupts  */ 
36 

37 

38  /*  defines  for  Vector  base  register  entries  */ 

39  #define  VB AJTTMER  0xffe40 1 30      /*  Vector  table  address  for  Timer-5  ISR  */ 

40  #define  VB A_PI      0xffe40 118      /*  Vector  table  entry  for  IP  interrupt  */ 
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41 
42 

43  /* * 

44  *  defines  for  interrupt  steering  mechanism  * 

45  * */ 

46 

47  #define  ISM_TIMER  0xfff48004     /*  ISM  Configuration  Register  for  Timer  A  */ 

48  #define  ISM_PI      0xfff48008      /*  ISM  Configuration  Register  for  PI       */ 

49  #define  ISM_SERIAL  0xfff48001      /*  ISM  Configuration  Register  for  serial  IO  */ 
50 

5 1  #define  APP_ICR     0xfff4800c      /*  abort/processor  interrupt  control  register  */ 

52 

53 

54 

55  /* * 


56  *  Base  Addressees  for  accessing  Parallel  IO-Ports    * 

57  * */ 


58#definePIO!   CTRL  0xfff40003 


59#definePIO!   DATA  0xfff40000 


60  #define  PI02  CTRL  0xfff40007 


61  #definePI02  DATA  0xfff40004 


62 


/*  control  register  for  PIO-1  */ 
/*  data  register  for  PIO- 1  Port  A  */ 

/*  control  register  for  PIO-2  */ 

/*  data  register  for  PIO-2  Port  A  */ 
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63 

64  /* * 

65  *  Base  Addressees  for  68030  Input/Output  Program     * 

66  *  as  outlined  in  Taurus  Manual,  Chapter  6  * 

67  * */ 


68  #define  IOP  CMDBLK     OxffeOOOOO 


69#defineIOP  START       0x01 


/*  address  for  IOP  Command  Block  */ 


/*  command  to  start  IOP  */ 


70  #define  IOP_STOP        0x00  /*  command  to  stop  IOP  */ 

71  #define  IOP_COMPLETE    0x80  /*  mask  for  operation  complete  */ 
72 


73  #define  IOPB_CONFIGURE  OxeO 

74  #define  IOPB_UNIT_OMNI0  0x10 

75  /* 


/*  command  to  configure  IOBP  */ 
/*  unit  #  for  omnimodule  #0  */ 


76  *  definitions  for  68030  Input/Output  Program,  (Leo,  05/13/97)  * 

77  * */ 

78 
79 

80  /*  Input/Output  Parameter  Block  structure,  according  Taurus  Manual,  p.  6-4  */ 

81  typedef  struct  { 

82  unsigned  char  cmd;    /*  command  */ 

83  unsigned  char  error;  /*  error  status  */ 

84  unsigned  short  options;    /*  options  */ 
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85  unsigned  short  reserved;  /*  reserved,  do  not  use  */ 

86  unsigned  char  unit;        /*  unit  number  */ 

87  unsigned  char  destUnit;    /*  destination  unit  */ 

88  unsigned  long  blockNumber;/*  logical  Block  number  */ 

89  unsigned  long  txCount;     /*  Transfer  count,  #  of  bytes  to  transfer  */ 

90  unsigned  long  *ptrSrc;     /*  address  of  source  */ 

91  unsigned  long  *ptrDst;     /*  Address  of  destination  */ 

92  }IOPB; 
93 

94  /*  Command  Block  structure  according  to  Taurus  Manual,  p.  6-3  */ 

95  typedef  struct  { 

96  unsigned  char  cmd;  /*  status  and  command  register  */ 

97  unsigned  char  reserved[3];    /*  not  yet  used  */ 

98  IOPB  *ptrToIOPB;     /*  pointer  to  IOBP  */ 

99  }CMD_BLOCK; 
100 

101 

102/*  Omnimodule  support  block  structure  according  to  Taurus  Manual  p.  6-12  */ 

103 typedef  struct  { 

104  unsigned  long  options;      /*  4  bytes  options,  unused  */ 

105  unsigned  long  *ptrlnit;     /*  pointer  to  initialization  routine       */ 

106  unsigned  long  *ptrTask;     /*  pointer  to  task  */ 
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107    unsigned  long  *ptrlntr;     /*  pointer  to  interrupt  servicing  routine  */ 

108}OSB; 

109 

1 10IOPB  iopbOMNIO;  /*  IOBP  for  Omnimodule  0  (used  for  serial  I/O  to  VT100  */ 

1HOSB    osbOMNIO;    /*  OSB  for  Omnimodule  0  (used  for  serial  I/O  to  VT 1 00  */ 

112 

1 13/* */ 

1 14/* */ 

115 

1 1 6unsigned  int  intCounter,  testCounter;     /*  count  the  interrupts  */ 

1 17unsigned  int  demo;  /*  switch  to  run  demo  see  driver()  in  movement.c  */ 

1 18unsigned  short  timer_in_ms;    /*  desired  timer  period  in  ms  */ 

119 

120 

121/* * 

122  *  definitions  for  inertial  measurement  routines  (imu.c)  * 

123  * */ 

124 

125/*  added  10  Sep  97  */ 
126typedef  struct  { 

127  unsigned  short  ax;  /*  linear  acceleration  in  x-direction  */ 

128  unsigned  short  ay;  /*  linear  acceleration  in  y-direction  */ 
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129  unsigned  short  az;  /*  linear  acceleration  in  z-direction  */ 

130  unsigned  short  omega_z;  /*  angular  velocity  in  z-direction  */ 
131JIMU; 

132 

1 33IMU  imu;  /*  stores  most  recent  JJVTU  data  (updated  with  */ 

134  /*  every  10ms  timer  interrupt  */ 

135 

1 36/* * 

137  *  definitions  for  Joystick  Control,  (Leo,  05/10/97)  * 

138  * */ 

139 

140typedef  struct  { 

141  double  x;  /*  x  position  (or  velocity)  */ 

142  double  y;  /*  y  position  (or  velocity)  */ 

143  double  omega;        /*  angular  velocity  */ 

144  unsigned  char  state;  /*  status  of  parallel  port  1,  channel  C  */ 
145JJPOINT; 

146  JPOINT  joystick;  /*global*/ 

147 

1 48typedef  struct  { 

149  double  x; 

150  double  y; 
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151 }  point; 

152 

153 

1 54typedef  struct  { 

155  point  coord; 

156  double  heading; 

157  double  kappa; 
158 }  Configuration; 

159  Configuration  vehicle;  /*global*/ 
160 

1 6 1  typedef  struct  { 

162  double  Speed; 

163  double  Theta; 

164  double  Omega; 
1 65 }  vehicleMotion; 

166  vehicleMotion  motion,motionO;  /*global*/ 

167 

168 typedef  struct  { 

169  double  rho; 

170  double  alpha; 
171 }  polar; 

172 
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173 

174/* */ 

175/*  definitions  for  wheel  control  */ 
176/* */ 

177 

178/*  write  these  masks  to  VME27 10  at  address  OxffffffOO  in  order  to  make      */ 

179/*  the  specific  motor  drive!  May  wish  to  logical  OR  with  previous  settings  */ 

180 

1 8 1  #define  TURN_FR     0x00004000     /*  turn  wheel  1  (front  right)  */ 

182#defineTURN_FL     0x00020000     /*  turn  wheel  2  (front  left)  */ 

183#defineTURN_RR     0x00100000     /*  turn  wheel  3  (rear  right)  */ 

184#defineTURN_RL     0x00800000     /*  turn  wheel  4  (rear  left)    */ 

185 

186#define  DRIVE_FR    0x00000004      /*  drive  wheel  1  (front  right)  */ 

187#defineDRIVE_FL    0x00000020     /*  drive  wheel  2  (front  left)  */ 

188#defineDRIVE_RR    0x00000100     /*  drive  wheel  3  (rear  right)  */ 

189#defineDRIVE_RL    0x00000800     /*  drive  wheel  4  (rear  left)    */ 

190 

1 9 1  #define  ALL_WHEELS  0x00924924      /*  select  all  wheels  for  turning  */ 

192  /*  and  driving  */ 
193 

194/*  — * 

219 


1 95  *  function  definitions  * 

196  * */ 

197 

198 void  setVME(unsigned  char  *board,  unsigned  char  data); 

199 void  toggle VME(unsigned  char  *board,  unsigned  char  data); 

200void  initBoards(void); 

201  void  piTest(void); 

202void  piHandler(void); 

203 void  advanceCount(); 

204  /*  global  variable  to  make  joystock  coordinates  accessible  */ 

205 

206#define  ARRAY_SIZE        4 

207#define  DegreesToRads     0.0174532925 

208#define  RadsToDegrees     57.2957795 1 308232 

209#define  DeltaT  0.01 

210 

211 

2 1 2double  desiredAngleRates[ ARRAY_SIZE], 

2 1 3  desired  AngleRatesOf  ARRAY_SIZE], 

214  desiredSpeeds_F[ARRAY_SIZE], 

2 1 5  desiredAngleRates_F[  ARRAY_SIZE], 

216  desiredSpeeds[ARRAY_SIZE], 

220 


217  actualSpeeds[ARRAY_SIZE],      /*  28  May  ejm  */ 

218  actualAngleRates[ARRAY_SIZE], 

219  DigitToCmDrive[ARRAY_SIZE], 

220  Display_Speeds[ARRAY_SIZE], 

221  Display_Steers[ARRAY_SIZE], 
222 

223  desiredAngles[ARRAY_SIZE], 

224  desiredAnglesO[ARRAY_SIZE], 

225  actual  Angles[ARRAY_SIZE]; 
226 

227  short  Steer_Digits[ARRAY_SIZE], 

228  Speed_Digits[ARRAY_SIZE]; 
229 

230double  WheelDriveAct[ARRAY_SIZE], 

231  WheelDriveDes[ARRAY_SIZE]; 

232 

233unsigned  long  int     WheelDriveActO[ARRAY_SIZE], 

234  WheelDriveActl  [ARRAY_SIZE], 

235  driveReadings[ARRAY_SIZE]; 
236 

237double  WheelDirAct[ARRAY_SIZE], 

238  WheelDirDes[ARRAY_SIZE], 

221 


239  PreviousCountSpeed[ARRAY_SIZE], 

240  PreviousCountSteer[  ARRAY_SIZE] ; 
241 

242unsigned  long  int     WheelDirActO[ARRAY_SIZE], 

243  WheelDirActl[ARRAY_SIZE], 

244  steerReadings[ARRAY_SIZE]; 
245 

246int  mode, 

247  oldMode, 

248  modeOstate, 

249  mode5state, 

250  modeTstate, 

251  Flag, 

252  oldFlag, 

253  edCounter, 

254  hallSensor3; 

255unsigned  int  intCounter,  testCounter;     /*  count  the  interrupts  */ 

256 

257 

258 

259/*unsigned  long  int  */ 

260double  previousCount,previousCountSteer,  Omega_Speed, 
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261previousCountSpeed;/*previousCount  represents  infinity  */ 

262 

263 double  K1[ARRAY_SIZE], 

264  K2[ARRAY_SIZE], 

265  K3[ARRAY_SIZE], 

266  K4[ARRAY_SIZE], 

267  K6[ARRAY_SIZE];      /*  slope  based  on  input  units  vs  output  velocity,    */ 

268  /*  input  range  from  0-  1020,  feedback  constant       */ 

269  /*  K3  is  the  inverse  of  (86.975velocity/1020  digit)  */ 
270 

271 

272#endif 

273 

275  End  of  shepherd. h 

277 

278 

279  #ifndef  _MOVEMENT_H_ 

280#define  _MOVEMENT_H_ 

281 

282#include  "shepherd. h" 
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283 


284 


285#define  PI 


3.14159265358979323846 


286#define  DPI 


6.28318530717958647692  /*  PI*2 


287#define  HPI 


1.570796327 


/*PI/2 


288#define  QPI  0.785398163  /*  PI/4 

289#define  QPIby500         0.0015707963 

290  /*  QPI/(5  seconds/deltaT)  */ 

291 

292 

293 

294 

295 double  wheel_speed[4],  wheel_angle[4]; 

296 

297void  initMovement(); 

298void  setupPolar(polar  []); 

299void  wheelMotion(); 

300void  bodyMotion(); 

301  void  driver(); 

302  void  joystickMotionlnterface(void); 
303 

304 
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305extern  double  desiredAngleRates[], 

306  desiredSpeeds[], 

307  PreviousCountSpeed[], 

308  PreviousCountSpeed[], 

309  PreviousCountSteer[]; 
310 

311polar  whp[4]; 

3 12double  pathLength,thetaDot,omegaDot,speedDot; 

313 

314 

315 

317 

318 

319double  sigma; 

320double  radius; 

321 

322double  ai[4],  bi[4]; 

323typedef  struct  { 

324  Configuration    config; 

325  point  center; 

326  double  radius; 
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327  double    a; 

328  double    b; 

329  double    c; 
330}  LINE; 

331 

332static  LINE  currentPath;  /*  holds  the  current  path  element  values  */ 

333 Configuration  incrementalMotion,  holdVehicle; 

334static  double  deltaS; 

335 

336void  tangentialMotion(); 

337void  circularArc(double  length,  double  alpha); 

338void  defineConfig(double  x,double  y,double  theta,double  kappa); 

339void  compose(); 

340double  steer(); 

341  void  constants(); 

342double  Psi(point  pi, point  p2); 

343double  distance(point  pi, point  p2); 

344void  initTangent(); 

345 

346#endif 

347 

348 
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349 

350 

35 1  #ifndef  _MOTOR_H_ 

352#define  _MOTOR_H_ 

353 

354#include  "shepherd.h" 

355 

356/* * 

357  *  Base  Addressees  for  accessing  Servo  Control  Cards  * 

358  *  Used  in  Home  Testing 

359  * */ 

360#define  SteerDrivelnteract  .02     /*  used  to  give  stability  to  wheel      */ 

361 

362#define  RadRateTodigit  195.3789    /*  digit/radpersec*/ 

363 

364#define  digitToRadDrive  -6.015495746e-5 

365  /*  driving  constant  rad/count  =  DPI/ 104450      May  8  */ 

366  /*  Experimental  Results  by  Ed  Mays  May  7  */ 

367  /*  Wheel  1  count  =  104456  */ 

368  /*  Wheel  2  count  =  104435  */ 

369  /*  Wheel  3  count  =  104454  */ 

370  /*  Wheel  4  count  =  104455  */ 
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371  /*  Average  count  =  104450  */ 

372  /*  cf.  2048  *  51  =  104448  */ 
373#define  digitToCmDrive  0.001 1369287 

374  /*  driving  constant  cm/count  =  digitToRadDrive*  18.9cm  5/8/97  */ 

375 

376#define  digitToRadSteer  -6.81 769239  le-5 

377  /*  steering  constant  rad/count  =  DPI/(2048*45)   19  Apr  */ 

378 

379#define  SteerFBGain  0.000;         /*  steering  feedback  gain  */ 

380#define  DriveFBGain  0.000;        /*  driving  feedback  gain  */ 

38 1  #define  DigitsHigh  1 023 

382#define  DigitsLow  -1024 

383#define  WheelRadius  18.9  /*  prev  def  in  cm  */ 

384#define  VME9210     0xffff0400     /*  Base  Address  analog  out  to  servo      */ 

385#define  VME9421     OxffffOOOO     /*  Base  address  data  in  from  servo       */ 

386#defineVME2170     OxffffffOO     /*  Base  address  data  out  to  servo        */ 

387#defineVMECTRl     0xffff6000     /*  Counter  */ 

388#define  K5  87.4  /*  control  feedback  constant  (cm/sec)  variable  28  May  ejm  */ 

389#define  DriveFeedBackGain  0.8     /*.8  control  drive  feedback  gain  28  May  ejm  */ 

390#define  angularK3  0.96963  /*  digit/rotational  speed  (rad/sec)     */ 

391#define  steerFeedbackGain   100.0  /*  steering  Feedback  gain  */ 

392#define  angularK5  5.23598 

228 


393#define  angleFeedbackGain   1000.0 

394 

395extern  unsigned  char  clrLine[6];  /*  ESC-Sequence  for  clear  line  */ 

396extern  char  bwheeldrivecdString[];/*  defined  in  shepherd.c  */ 

397extern  unsigned  char  bcdString[]; 

398 

399double  Drive_Feedback[ ARRAY_SIZE] ; 

400 


401  extern  double  desiredAngleRates[], 

402 

desiredSpeeds[], 

403 

PreviousCountSpeed[], 

404 

PreviousCountSteer  [], 

405 

DigitToCmDrive[], 

406 

Display_Speeds[], 

407 

Display_Steers[], 

408 

desiredAngles[], 

409 

actualAngles[]; 

410 

411/*  — 

* 

412  *  function  definitions  * 

413  * */ 

414void  driveSpeed(short  []); 
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41 5 void  driveSteer(short  []); 

416void  driveMotors(); 

4 17 void  wheelDrive(void); 

418voidallStop(void); 

419void  updateEncoders(void); 

420 void  updateWheelDrive(void); 

421  void  updateWheelSteer(void); 

422 void  displayDirections(void); 

423void  displaySpeed(void); 

424int  readSteerEncoders(unsigned  long  int  []); 

425void  testDrive(void); 

426void  readEncoders(void); 

427void  accumulatedriveSpeed(); 

428 void  displayDriveAngle();  /*  added  15  may  */ 

429void  drivingFeedback();  /*  28  May  ejm    */ 

430double  velocityReferenceTable(double,int);  /*  28  May  ejm    */ 

431  void  steeringFeedback();  /*  4  June       */ 

432double  rateReferenceTable(double);  /*  4  June       */ 

433void  computeActualRates();  /*  5  June       */ 

434int  convertDifference(int);  /*    1 1  June  ejm  */ 

435 

436void  alignWheels(void); 
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437void  clearShaftEncoder(unsigned  short  motors); 

438void  readWheelStatus(unsigned  char  *array); 

439 

440#endif 

441 

442 

443 

444/*  Timer.h  */ 

445 

446#ifndefTIMER_H 

447#define  TIMER_H 

448 

449/*  Defines  for  Timer  control  */ 

450#define  TIMER_CTRL  0xfff4 1 002      /*  Control  register  for  Timer  A  */ 

45 1  #def ine  TIMER_D ATA  0xfff4 1 000     /*  Data  register  for  Timer  A  */ 

452 

453 

454/*  settings  master  mode  register  according  to  fig.  1-12  */ 

455#define  TIMER_MASTER_MODE  OxbafO  /*  timer  master  mode  register  */ 

456  /*  b=BCD  count,  16  Bit  data  bus  */ 

457  /*  4=divide  by  4  */ 

458  /*  8  =  Source  F4  (divide  by  1000)        */ 
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459  /*  0  =  don't  care  */ 

460 

461 

462/*  settings  for  counter  mode  register  according  to  fig.  1-17  */ 

463#define  TIMER_MODE  0x0f3 1  /*  Counter  Mode  Register  Bit  Assignment  */ 

464  /*  0  =  no  gating,  count  on  rising  edge  */ 

465  /*  8  =  Source  F4  (divide  f  by  1 000)      */ 

466  /*  3  =  BCD  repetitive  count,reload  load  */ 

467  /*  2  =  count  down,  toggle  TC  */ 

468  /*  or  1  =  count  down,  active  high  Terminal  Count  Pulse  */ 
469 

470/* * 

471  *  function  definitions  * 

472  * */ 

473 

474  void  TimerHandler(void); 

475  void  timerStart(void); 
476 

477 
478 

479#endif 
480 
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481 

482  End  of  timer.h 
483 

484 

485 

486 

487 

488#ifndef  _MATH_H_ 

489#define  _MATH_H_ 

490 

491 

492 

493double  cos(double  x); 

494double  sin(double  x); 

495double  atan2(double  x,  double  y); 

496double  atan(double  x); 

497double  new_sqrt(double  x); 

498double  new_abs(double  x); 

499double  norm(double  angle); 

500double  min  (double,  double); 
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501  double  max  (double,  double); 

502#endif 

503 

504 

505/* * 

506  *  * 

507  *  File:  S  E  R  I  A  L  .  H  * 

508  *  * 

509  *  Environment:  GCC  Compiler  v2.7.2  * 

510  *  Last  update:   13  March  1997  * 

5 1 1  *  Name:  Thorsten  Leonardy  * 
512*  Purpose:  Header  File  for 'serial. c'  * 
513  * */ 

514 

5 1 5#ifndef  _SERIAL_H_ 

5 1 6#define  _SERIAL_H_ 

517 

518 

5 1 9#define  CONSOLE     0xfff4a000     /*  Base  address  68C68 1  DUART  */ 

520 

521/* * 

522  *  function  definitions  * 
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523  * */ 

524 

525  void  inPortAHandler(void);  /*  interrupt  handler      */ 

526  void  siolnit(void);  /*  initialize  DUART       */ 

527  void  sioOut(int  hostFlag,  unsigned  char  *s);  /*  Output  a  string        */ 

528  void  gotoXY(int  x,  int  y);  /*  position  cursor        */ 
529 

530#endif 
531 

532/********************************************************************** 
533  End  of  serial.h 

535 
536 
537/* * 

538  *  * 

539  *  File:  U  T  I  L  S  .  H  * 
540*                                                                             * 

541  *  Environment:  GCC  Compiler  v2.7.2  * 

542  *  Last  update:   13  March  1997  * 

543  *  Name:  Thorsten  Leonardy  * 

544  *  Purpose:       Header  File  for  'utils.c'  * 
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545  * */ 

546 

547#ifndef  _UTILS_H_ 

548#define  _UTILS_H_ 

549 

550/* * 

551  *  function  definitions  * 

552  * —  */ 

553 

554 void  setVME(unsigned  char  *board,  unsigned  char  data); 

555void  toggleVME(unsigned  char  *board,  unsigned  char  data); 

556void  initBoards(void); 

557void  piTest(void); 

558void  piHandler(void); 

559/*void  readClock(void);  */ 

560/*void  WRITE_CLOCK(void);  */ 

561 

562/*  Modified  18  Apr  */ 

563unsigned  char  b2a(int); 

564void  b2a2(unsigned  char  *,  unsigned  char); 

565void  convertToASCII(unsigned  int  ndigits,  unsigned  int  data,  char  *str); 

566void  readJoyStick(void); 
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567double  insensitive(double  z); 

568 

569#endif 

570 

572  Endofutils.h 
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APPENDIX  K:  SHEPHERD  OPERATING  MANUAL 

OVERVIEW 

The  Purpose  of  this  document  is  to  provide  a  quick  guide  for  doing  downloads  for  testing 
or  other  purposes.  For  a  more  detailed  guide  see  the  Shepherd  Operators  Guide  (SOG). 

The  Shepherd  compilation  and  download  process  is  a  four  step  process: 

-  Compile  executable  on  workstation. 

-  FTP  S-Records  to  laptop. 

-  Use  Windows  95  HyperTerminal  program  for  direct  connection. 

-  Run  the  program  once  download  complete. 


Compile  Executable  on  Workstation 

1 .  Once  you  have  logged  in  on  the  Shepherd  account,  then  use  the  xinit  command  to 
generate  the  X-Windows  environment. 

2.  In  the  large  terminal  window  type  "cap"  at  the  UNIX  prompt  and  press  the  return  key. 

3.  The  alias  "cap"  logs  you  onto  capella  (the  standard  login  script  will  scroll  by).  The 
Shepherd  group  uses  capella  (server)  because  of  the  nature  of  the  cross  compilation  used 
for  the  "Taurus  board"  and  Motorola  68040  CPU. 

4.  Next,  in  the  large  terminal  window  type  "taurus"  at  the  prompt  and  press  the  return 
key. 

5.  The  alias  "taurus"  sets  up  the  environment  for  compilation  and  print  services. 

6.  Next,  in  the  large  terminal  window  type  "cd  srk"  at  the  prompt  and  press  the  return 
key;  this  takes  you  to  the  Shepherd  Real-time  Kernel  (~shepherd/srk)  directory.  While  in 
srk  you  can  modify  or  edit  the  require  files  with  your  favorite  editor  (e.g.,  xemacs  or 
nedit).  Once  you  have  completed  your  work,  save  your  files  and  compile.  Seefigurel  on 
the  next  page. 
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cmdtool  -  /bin/eth 


T^r 


ft  mac  \   mo  vox 


shepnerdKap 

•••*••••••   WAPNTNG    ********** 

UHAUTHORIHED  USE  OF  THIS  DEPART>€NT  OF  DEFENSE   (DOC)   INTEREST 
COffUTER  SYSTEM  AND/OR  SOFTWARE   IS  PROHIBITED  BY  PUBLIC   LAW. 

This  1s  a  Department  of  Defense  co»puter  system.     THIS  COMPUTER 
SYSTEM,    INCLUDINC   ALL   RELATED  EQUIPMENT,    NETWORKS   AND  NETWORK 
DEVICES   (SPECIFICALLY  INCLUDINC  INTERNET  ACCESS),   ARE  PROVIDED 
ONLY   FOR  AUTHORIZED  U.S.    GOVERNMENT  USE.      DC©  computer  systems 
■ay  be  monitored  for  all    lawful   purposes.  Including  to  ensure 
that  their  use  1s  authorized,  for  management  of  the  systei,  to 
facilitate  protection  against  unauthorized  access,  and  to  verify 
security  procedures,  survivability  and  operational  security. 
Monitoring  Includes  active  attacks  by  authorized  DOD  entities 
to  test  or  verify  the  security  of  this  system.     During 
monitoring,  information  lay  be  examined,   recorded,  copied  and 
used  for  authorized  purposes.     All   Information,  including 
personal   information,  placed  on  or  sent  over  this  system  may 
be  monitored.      USE   OF  THIS  DOD  COMPUTER  SYSTEM.   AUTHORIZED  OR 
UNAUTHORIZED.   CONSTITUTES   CONSENT  TO  MONITORING  OF  THIS   SYSTEM. 

Evidence  of  unauthorized  use  collected  during  monitoring  may 
be  used  for  administrative,  criminal   or  adverse  action.     USE  OF 
THIS  SYSTEM  CONSTTTUTES  CONSENT  TO  MONITORING   FOR  THESE  PURPOSES. 

•••**•  CLASSIFIED   PROCESSING  ON  THIS  SYSTEM  IS  PROHIBITED  *••••• 
Last  login:   Sat  Jul   19  12:37:56  from  europa.cs.nps.na 


Welcome  to  the  Computer  Science  Department 
Sat  Jul  19  00:30:01   PDT  1997 
You  have  mall . 
shepherdnaurus 
shepherd>cd  srk 
/users/ res/shepherd/srk 
shepherd^ 


n!c  tort  t'vpit  Options  Buffers  Toots  C  Mode  Commands 


J  ■wl.mri  w  1  ml  rSi  vattl^m  \mm\  Hi  ,i»,I«£Jj£hl 


JincLudr  'shepherd,  b" 

••include  "movement  h* 

•^include  '»och  h" 

-■include  'serve  h' 


mxtmrn  double  Drivt_St«mrJrmlttmm|l, 

0  r  i ve  _Spe*d_ Value  ■  j  | , 
PrtviOuaCountSpMd;  I .. 
FreviousCountSpeedi  j, 
PrmvioumCountStmrnt [ I j 

polar  vhp[4]. 

double  pathLsngth,  tbetmDot.  oamg-mDot,  •peedDo t . 

void  driver () 
( 

remdCncodersO.       /•     Read  Brivc/Steor  Kotocs       •/ 

o  oxputemo  tuelRmtme  <)  j 

/'accumal*t*rtnvf'$p*edO  .        only  for  vheel  speed  display iacj  »/ 

accumulet*DnveStetr() , 

update?  () ; 

calculate  O  i 

/*      tsmtSrivmlO,     */ 

convex  tLikitQj 

advancaCoufttOi 


*  Initialise  Kovc»cnt 

*  mtiolico  Coitf  igruration  end  vehicle  motion 

void  inic*©v**entO 
( 

int  ix. 
Mode  -  l 
o»*o»_>peed-a  0; 
teetCounter-O, 
•  dCotmwr-0. 

?athLenath*0.0. 
reviouoCountSpeed  1 0 1 *99999999; 
rrvviousCountSpeedj 1 ) -99999999 . 
JPrmvKnieCountSpead  f  2 1  -99999999. 
PrevioueCountSpeed  j  3 ) -99999999. 
?rmvi*uaCountSt*er  |01  -99999999, 
PreviouaCauntSteec  I1U99999999. 
-JJKmmce:  aw.c  (C  Font)— -- Top------- 


fontirying  move  c.        dons 


/bin/ah 


Figure  1 :  The  Unix  Workstation  Environment 

7.  Compilation  is  done  through  the  use  of  a  makefile.  Hence,  to  compile  all  you  must  do 
is  type  "make  comp"  at  the  prompt  and  press  the  return  key  (this  will  either  succeed  or 
fail).  If  the  compilation  fails  work  the  errors  provided  by  the  compiler  and  compile  again 
(an  iterative  process).  Once,  the  compilation  is  a  success  you  are  ready  to  FTP  the  S- 
records  to  the  laptop. 


FTP  S-Records  to  Laptop 

8.  To  begin  to  FTP  the  S-records  to  the  laptop  a  few  items  must  be  accomplished.  First, 
the  robot  power  must  be  switched  on  (levers  a,  b,  and  c  on  the  power  supply  in  the  "up" 
or  closed  position  on  the  physical  robot;  provides  power  to  the  robot  and  charges  the 
batteries). 
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See  figure  2  (note  the  "up"  position  below  represents  the  down  position  on  the  physical 
robot). 

Shepherd  Rotary  Vehicle 
Power  Supply 


AC/DC  Converter 


-o      o cf       a 

bSq 


Shepherd 
Rotary  Robot 


Figure  2:  Power  Supply  Diagram 

9.  Secondly,  the  laptop  must  be  on  and  connected  to  the  local  network  via  the  PCMIA 
card  (ethernet).  Press  the  laptop  "On" button. 

10.  Ensure  the  laptop  powersupply  is  plugged  in,  and  connected  to  the  laptop. 

1 1 .  Ensure  the  therenet  cable  is  properly  connected  to  the  to  the  PC  card. 

12.  After  booting  our  laptop  will  prompt  you  to  login  as  guest— just  "click  on  the 
cancel"  button.  You  should  see  the  Windows  desk  top  on  the  laptop  (figure  3  below). 
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^•^    1:20  AM 


Figure  3:  Windows  Desk  Top 


13.  The  screen  print  below  describes  the  way  the  windows  should  look.  Now  double 
click  on  the  WS_FTP95  shortcut  to  open  the  ftp  tool. 
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My  Computet  My  Briefcase 


Network 
Neighborhood 

Shortcut  to 
Telnet 

W\m 

■  ,* 

Inbox 

9 

Shortcut  (3)  to 
Ping 

•  1 

Recycle  Bin 

WS  FTP95 

The  Internet 

i IE 

M    sk 


The  Microsoft     ShortcuUo 
N*WOfk      ses2download 


^  Start 


JN&    1:20  AM 


Figure  4:  Windows  WS_FTP95  Shortcut 

14.  At  this  point  the  ftp  tool  opens.  Click  on  the  "OK"  button.  All  information  has  been 
previously  set  for  you(e.g.,  hostname,UserID,  and  Password).  See  figure  5. 

15.  The  next  Window  has  a  split  panel  showing  the  Remote  System  (workstation)  and 
the  Local  System  (the  laptop).  The  directories  have  been  saved,  so  they  always  open  to 
the  correct  directories.  The  file  to  be  ftp'd  is  shepherd.TXT,  it  will  be  ftp'd  from  the 
remote  system  (~shepherd/srk)  to  the  local  system  (c:\shepherdump).  The  file 
shepherd.TXT  contains  the  S-Records  that  will  be  eventually  downloaded  to  the  actual 
robot  CPU.  To  accomplish  the  ftp  highlight  the  file  to  be  transferred  with  your  mouse 
and  click  on  the  arrow  that  points  left  (See  figure  6).  The  file  transfer  usually  takes 
about  .3  seconds. 
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miWS_FTP95  LE 


—Local  System- 


HK63 


C:\Proaram  FilesWS  FTI^I 


-Remote  System- 


t 

Qj  test, 
g  comp. 
fJU  conni 
M  erro: 
[H  licei 
[JH  prorc 
[=|  what: 
M  WS_F' 
D  WS_F 
H  WS_F 
U  US_F' 
1]  US_F' 
a  [-a- 
1J I 


Session  Profile 

Profile  Name:jSJ5! 

d 

Detete               Save 

Neja 

HostMame:|l31.120.1.13 

Host  Iype:j  Automatic  detect 

d 

User  ID:  jshepherd 

Password;  | -"""'""' 

Account:] 

Initial  Directories 

Remote  Host:  j/tmp_mnt/'usersAes/'shepherd/ 

Local  PC:  j  C:  \shepherdump 

Ok 


Cancel 


Advanced... 


Help 


I-  Anonymous  t.pgjn 
W  Save  Password 
I*  Auto  Save  Config 


H 


ChgDsr 
MkDr 


View 


Exei 


fiensTi* 


Delet, 


Refresh 


Di-itro 


■ 


Comment 


WINSOCKDLL:  Microsoft  Windows  Sockets  Version  1.1. 

WS.FTP95  LE  4.04  98.08.21 ,  Copyright  ®  1 932-1 996  Ipswitch,  Inc.  AI  rights  reserved 


Connect 


Cancel 


LogWnd 


Help 


Options 


About 


i^Q  Start 


FT? WS_FTP95  LE 


Figure  5:  Windows  WS_FTP95  Tool 


Exit 


\j$%    1:21AM 
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i^B  Start    FTP  WS_FTP95  LE  131.1 


1:23  AM 


Figure  6:  WS_FTP95  Tool  File  Transfer 


16.  The  file  is  now  on  the  "hard"  disk  of  the  laptop.  You  can  now  close  the  window  or 
kill  the  process  by  clicking  on  the  appropriate  button  (active  window:upper  right  corner 
area  "X"). 
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Use  Windows  95  HyperTerminal  Program  for  Direct  Connection 

17.  You  are  now  back  at  the  Windows  desk  top.  Now  double  click  on  the  ses2download 
shortcut  to  open  a  hard-line,  under  Windows  95  HyperTerminal  (See  figure  7). 


My  Lomputer  My  Briefcase 


Network 
Neighborhood 


Shortcut  to 
Telnet 


Shortcut  (3)  to 
Inbox  Rrig 


Recycle  Bin 


The  Internet 


rWSFTPSS 


m  si. 


TheMicrosoft     shortcut  to 
Network      $es2do,A,n|oad 


i^f]  Start 


J^    1:20AM 


Figure  7:  Windows  ses2download  Shortcut 
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18.  The  next  window  to  appear  will  be  the  open  HyperTerminal  window  (See  Figure  8). 
Press  the  "reset"  button  on  the  OMNIBYTE,  Taurus  board.  The  Taurus  bug  (debugger) 
prompt  will  appear  in  the  HyperTerminal  window  (See  Figure  8). 


<*;  ses2download  -  HyperTerminal 


File    Edit    View    £al    Transfer    Help 


sua 


D 


G» 


m  S  ^B  nf 


3 


Taurus   Debugger /Diagnostics   Version   3.2 
FPC   passed  test 
Taurus_Bug>bf    0    40000    0 
Effective    address:    00000000 
Effective    address:    0003FFFC 
T  aurus_Bug> 

lo 


11/28/95 


i 


.  j  rxnrt.c-i 


it/T-W 


Kwvi  n  n 


ii  IcrDi'n  i  ir  adc       1>.h  iu      !  r  — i.  „_        foZTmUT" 


^B  Start  |  ?TpWS.FTP95LE  131 .120.1....||%ses2download  -  Hype 


|#%    1:25AM 


Figure  8:  Windows  HypertTerminal  Window 


19.  Ensure  the  lever  on  the  switch  box  is  placed  on  console  (this  allows  the  console  to 
emulate  a  VT220). 

20.  At  the  Taurus  bug  prompt  type  "bfO  40000  0"  and  press  the  enter  key.  This 
command  is  called  block  fill  by  the  debugger  it  allows  you  to  disable  the  parity  error 
interrupt  (PET)  and  prevents  problems  caused  by  uninitialized  variables.  See  Figure  8. 

21.  At  the  Taurus  bug  prompt  type  "lo"  and  press  the  enter  key.  The  "lo"  command 
initiates  the  download  from  the  console.  See  Figure  8. 
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22.  Next  place  the  lever  on  the  switch  box  is  placed  on  host  (this  makes  possible  the  use 
of  the  RS232  protocol  to  download  shepherd.TXT  from  c:\shepherdump  to  the  Taurus 
board). 

23.  Click  on  the  HyperTerminal  "Transfer  "option  and  choose  the  "Send  Text  File". 
All  the  "Send  Text  File"  parameters  have  been  previously  set,  so  there  is  no  action  to 
take  in  that  regard. 

See  Figure  9. 


'*  ses2download  -  HyperTerminal 


£ie   £d*   View    £al 


HBE3 


Db  <m 


Taurus  Debugger I 
FPC  passed  test 
Taurus_Bug>bf  0 
Effective  address: 
Effective  address: 
Taurus_Bug> 

lo 


Help 

Send  Fie... 
Eeceive  Fie... 
Capture  Text.. 


Send  Test  File.. 


Capture  to  Fjthter 


<n   3.2  -  11/28/95 


00000000 
0003FFFC 


3 


A 


r 1.  .  t.m.A  a-  *_  i 


JBStart    Tp"WS_FTP95LE  131.120.1...    tfo  tes2download    Hype ... 


:3%    127AM 


Figure  9:  Windows  HypertTerminal  Window 
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24.  Now  move  to  the  root  directory  and  select  the  c:\shepherdump  directory,  and  double 
click  on  shepherd.TXT  file.  See  figure  10. 


*y  ses2download  -  HyperTerminal 


file    £dit    View    £aB    Jjansfer    Help 


Send  Text  File 


Look  jit 


£fj  shepherdump 


3  JH]  &1  Wm 


Fteijame:        {shepherd 


Fies  of  iype:     |  Text  file  ("TXT) 


"3 


Cancel 


HBE3 


I cx-oo  nut  lcrc.ni  i         Ipadc       (>.n  in      i r  ~^ i d.:„>  ~-»u~ 


If »- j  rvnj."n 


jgB Start |  FIPWS.FTP35LE  131.1 2ai....|^ «es2download  -  Hype... 

Figure  10:  Send  File  From  shepherdump 


L#%    1:29  MA 


The  download  process  is  in  motion.  The  "red"  transmit  light  on  the  RS232  connector  to 
the  switch  box  will  become  faint  while  transmission  is  in  progress.  Once  the 
transmission  is  complete  the  "red"  transmit  light  on  the  RS232  connector  to  the  switch 
box  will  become  a  constant  red;  the  Hyperterminal  window  will  pause  during  the 
transmission  process.  The  Taurus  bug  prompt  will  appear  in  the  HyperTerminal 
window  after  the  transmission  is  complete. 

25.  Ensure  the  lever  on  the  switch  box  is  placed  on  console  (this  allows  the  console  to 
emulate  a  VT220). 
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25.  Now  type  "go"  at  the  Taurus  bug  prompt  and  press  the  enter  key  (see  figure  1 1). 
The  program  that  you  have  previously  downloaded  will  be  executed. 


%:  ses2download  -  HyperTerminal 


Ffe    £dit    View    £al    Iransfer    Help 


B3 


D 


@  I    i6  9    iff 


3 


Taurus  Debugger /Diagnostics  Version  3.2  -  11/28/95 

FPC  passed  test 

Taurus_Bug>bf  0  40000  0 

Effective  address:  00000000 

Effective  address:  0003FFFC 

Taurus_Bug> 

lo 


Taurus_Bug> 


go 


I 


'  'wTinn  1  ivwi n  ni~i  "icroni  i         lr;.oc       Imi  it«      I r  _>...„        lo.:~i  ~-u- 


.j  rvrx*.  An 


IjgStartj  rTPWS.FTP95LEl31.12ai.... 


I  $es2do  wnload  -  Hype...  |jy^    1:31AM 

Figure  1 1 :  Taurus  Bug  Prompt  Returns  After  Transmission  Completion 
and  the  "go"  Command  is  given  to  Execute  the  Program. 
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After  the  "go"  has  been  given  and  the  execution  begins  the  Shepherd  Main  Menu 
appears  for  your  selection. 


4p  ses2download  -  HyperTeiminal 


Fife    Edit    M»w    £a!l    Iransfer    Help 


gm 


D 


G» 


sOQ    if 


"3 


SHEPHERD  Main  Menu  (Last  Update:  27  Feb  97) 
1 s o 5 0 S 0 5 0 S- 


0 S- 


-0 s o 5 0 
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Diagnostics 


(1)  Stop 

(2)  Straight  Motion  (Autonomous) 

(3)  Straight  Motion  by  Joystick 

(4)  XY-Motion  by  Joystick 

(5)  Rotate 

(6)  Sinusoidal  Motion 

(7)  Tornado  (External  Center  of  Rotation) 

(8)  Tornado  (Internal  Center  of  Rotation) 

(9)  Align  Wheels 
(0)  Exit  Program 


i 


'  icroni  i         Itaoc      Uu  iw      !  r  .w 


ir». „_..».  j  n.rtn.nn  K/ti 


i  Wflii  on* 
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Figure  12:  Shepherd  Main  Menu 


Note:  remember  this  is  a  quick  guide  and  does  not  provided  answers  to  questions 
concerning  file  size,  debugger  commands,  and  other  requirements  or  constraints. 
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APPENDIX  L:  SENSING  SIMULATION  DATA 


Distance       x  y  theta  psi 

0.000  0.400  0.000              0.001  2.355 

0.400  0.800  0.001              0.002  2.353 

0.800  1.200  0.002              0.005  2.352 

1.200  1.600  0.005              0.008  2.350 

1.600  2.000  0.008              0.011  2.348 

2.000  2.400  0.014               0.015  2.347 

2.400  2.800  0.021  0.020  2.345 

2.800  3.200  0.029  0.025  2.344 

3.200  3.600  0.041  0.031  2.342 

3.600  3.999  0.054  0.037  2.341 

4.000  4.399  0.070  0.043  2.339 

4.400  4.799  0.088  0.050  2.337 

4.800  5.198  0.110  0.057  2.336 

5.200  5.597  0.134  0.064  2.334 

5.600  5.996  0.161  0.072  2.333 

6.000  6.395  0.192  0.080  2.331 

6.400  6.794  0.225  0.088  2.330 

6.800  7.192  0.262  0.096  2.328 

7.200  7.590  0.302  0.105  2.326 

7.600  7.988  0.346  0.114  2.325 

8.000  8.385  0.393  0.123  2.323 

8.400  8.782  0.444  0.131  2.322 

8.800  9.178  0.498  0.141  2.320 

9.200  9.574  0.556  0.150  2.319 

9.600  9.969  0.617  0.159  2.317 
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10.000 

10.364 

0.682 

0.168 

2.315 

10.400 

10.758 

0.751 

0.177 

2.314 

10.800 

11.151 

0.823 

0.187 

2.312 

11.200 

11.544 

0.899 

0.196 

2.311 

11.600 

11.936 

0.979 

0.205 

2.309 

12.000 

12.327 

1.062 

0.214 

2.308 

12.400 

12.718 

1.149 

0.224 

2.306 

12.800 

13.107 

1.240 

0.233 

2.304 

13.200 

13.496 

1.334 

0.242 

2.303 

13.600 

13.884 

1.431 

0.251 

2.301 

14.000 

14.271 

1.532 

0.260 

2.300 

14.400 

14.657 

1.637 

0.269 

2.298 

14.800 

15.042 

1.745 

0.278 

2.297 

15.200 

15.426 

1.856 

0.286 

2.295 

15.600 

15.810 

1.971 

0.295 

2.293 

16.000 

16.192 

2.089 

0.303 

2.292 

16.400 

16.573 

2.210 

0.312 

2.290 

16.800 

16.953 

2.334 

0.320 

2.289 

17.200 

17.332 

2.461 

0.328 

2.287 

17.600 

17.711 

2.592 

0.336 

2.286 

18.000 

18.088 

2.725 

0.344 

2.284 

18.400 

18.464 

2.862 

0.352 

2.282 

18.800 

18.839 

3.001 

0.359 

2.281 

19.200 

19.213 

3.143 

0.367 

2.279 

19.600 

19.585 

3.288 

0.374 

2.278 

20.000 

19.957 

3.435 

0.381 

2.276 

20.400 

20.328 

3.586 

0.388 

2.275 

20.800 

20.698 

3.738 

0.395 

2.273 

21.200 

21.066 

3.893 

0.402 

2.271 
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21.600 

21.434 

4.051 

0.408 

2.270 

22.000 

21.801 

4.211 

0.415 

2.268 

22.400 

22.166 

4.373 

0.421 

2.267 

22.800 

22.531 

4.538 

0.427 

2.265 

23.200 

22.894 

4.704 

0.433 

2.264 

23.600 

23.257 

4.873 

0.438 

2.262 

24.000 

23.619 

5.044 

0.444 

2.260 

24.400 

23.980 

5.217 

0.449 

2.259 

24.800 

24.339 

5.391 

0.454 

2.257 

25.200 

24.698 

5.568 

0.459 

2.256 

25.600 

25.056 

5.746 

0.464 

2.254 

26.000 

25.414 

5.926 

0.469 

2.253 

26.400 

25.770 

6.108 

0.474 

2.251 

26.800 

26.126 

6.291 

0.478 

2.249 

27.200 

26.480 

6.476 

0.482 

2.248 

27.600 

26.834 

6.662 

0.486 

2.246 

28.000 

27.188 

6.849 

0.490 

2.245 

28.400 

27.540 

7.038 

0.494 

2.243 

28.800 

27.892 

7.229 

0.497 

2.242 

29.200 

28.243 

7.420 

0.501 

2.240 

29.600 

28.594 

7.613 

0.504 

2.238 

30.000 

28.944 

7.807 

0.507 

2.237 

30.400 

29.293 

8.001 

0.510 

2.235 

30.800 

29.642 

8.197 

0.513 

2.234 

31.200 

29.990 

8.394 

0.516 

2.232 

31.600 

30.338 

8.592 

0.518 

2.231 

32.000 

30.685 

8.790 

0.521 

2.229 

32.400 

31.032 

8.990 

0.523 

2.227 

32.800 

31.378 

9.190 

0.525 

2.226 
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33.200 

31.724 

9.390 

0.527 

2.224 

33.600 

32.070 

9.592 

0.529 

2.223 

34.000 

32.415 

9.794 

0.530 

2.221 

34.400 

32.760 

9.996 

0.532 

2.220 

34.800 

33.105 

10.200 

0.533 

2.218 

35.200 

33.449 

10.403 

0.535 

2.216 

35.600 

33.793 

10.607 

0.536 

2.215 

36.000 

34.137 

10.811 

0.537 

2.213 

36.400 

34.481 

11.016 

0.538 

2.212 

36.800 

34.824 

11.221 

0.538 

2.210 

37.200 

35.167 

11.426 

0.539 

2.209 

37.600 

35.511 

11.632 

0.540 

2.207 

38.000 

35.854 

11.837 

0.540 

2.205 

38.400 

36.197 

12.043 

0.540 

2.204 

38.800 

36.540 

12.249 

0.541 

2.202 

39.200 

36.883 

12.455 

0.541 

2.201 

39.600 

37.225 

12.661 

0.541 

2.199 

40.000 

37.568 

12.867 

0.541 

2.198 

40.400 

37.911 

13.073 

0.541 

2.196 

40.800 

38.254 

13.279 

0.540 

2.194 

41.200 

38.597 

13.484 

0.540 

2.193 

41.600 

38.941 

13.690 

0.539 

2.191 

42.000 

39.284 

13.895 

0.539 

2.190 

42.400 

39.627 

14.100 

0.538 

2.188 

42.800 

39.971 

14.305 

0.537 

2.187 

43.200 

40.315 

14.510 

0.537 

2.185 

43.600 

40.658 

14.714 

0.536 

2.183 

44.000 

41.002 

14.918 

0.535 

2.182 

44.400 

41.347 

15.122 

0.534 

2.180 
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44.800 

41.691 

15.325 

0.532 

2.179 

45.200 

42.036 

15.528 

0.531 

2.177 

45.600 

42.381 

15.730 

0.530 

2.176 

46.000 

42.726 

15.932 

0.528 

2.174 

46.400 

43.072 

16.133 

0.527 

2.172 

46.800 

43.418 

16.334 

0.525 

2.171 

47.200 

43.764 

16.535 

0.524 

2.169 

47.600 

44.111 

16.734 

0.522 

2.168 

48.000 

44.457 

16.934 

0.521 

2.166 

48.400 

44.805 

17.132 

0.519 

2.165 

48.800 

45.152 

17.330 

0.517 

2.163 

49.200 

45.500 

17.528 

0.515 

2.161 

49.600 

45.848 

17.724 

0.513 

2.160 

50.000 

46.197 

17.920 

0.511 

2.158 

50.400 

46.546 

18.116 

0.509 

2.157 

50.800 

46.896 

18.310 

0.507 

2.155 

51.200 

47.246 

18.504 

0.505 

2.154 

51.600 

47.596 

18.697 

0.503 

2.152 

52.000 

47.947 

18.889 

0.500 

2.150 

52.400 

48.298 

19.081 

0.498 

2.149 

52.800 

48.649 

19.272 

0.496 

2.147 

53.200 

49.002 

19.461 

0.493 

2.146 

53.600 

49.354 

19.650 

0.491 

2.144 

54.000 

49.707 

19.839 

0.489 

2.143 

54.400 

50.060 

20.026 

0.486 

2.141 

54.800 

50.414 

20.212 

0.484 

2.139 

55.200 

50.769 

20.398 

0.481 

2.138 

55.600 

51.124 

20.582 

0.478 

2.136 

56.000 

51.479 

20.766 

0.476 

2.135 
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56.400 

51.835 

20.949 

0.473 

2.133 

56.800 

52.191 

21.131 

0.470 

2.132 

57.200 

52.548 

21.311 

0.468 

2.130 

57.600 

52.905 

21.491 

0.465 

2.128 

58.000 

53.263 

21.670 

0.462 

2.127 

58.400 

53.621 

21.848 

0.460 

2.125 

58.800 

53.980 

22.025 

0.457 

2.124 

59.200 

54.339 

22.201 

0.454 

2.122 

59.600 

54.699 

22.376 

0.451 

2.121 

60.000 

55.059 

22.550 

0.448 

2.119 

60.400 

55.420 

22.723 

0.445 

2.117 

60.800 

55.781 

22.894 

0.442 

2.116 

61.200 

56.143 

23.065 

0.440 

2.114 

61.600 

56.505 

23.235 

0.437 

2.113 

62.000 

56.868 

23.403 

0.434 

2.111 

62.400 

57.231 

23.571 

0.431 

2.110 

62.800 

57.595 

23.738 

0.428 

2.108 

63.200 

57.959 

23.903 

0.425 

2.106 

63.600 

58.323 

24.067 

0.422 

2.105 

64.000 

58.689 

24.231 

0.419 

2.103 

64.400 

59.054 

24.393 

0.416 

2.102 

64.800 

59.420 

24.554 

0.413 

2.100 

65.200 

59.787 

24.714 

0.410 

2.099 

65.600 

60.154 

24.873 

0.407 

2.097 

66.000 

60.522 

25.030 

0.404 

2.095 

66.400 

60.890 

25.187 

0.401 

2.094 

66.800 

61.258 

25.343 

0.398 

2.092 

67.200 

61.627 

25.497 

0.395 

2.091 

67.600 

61.997 

25.650 

0.392 

2.089 
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68.000 

62.367 

25.802 

0.389 

2.088 

68.400 

62.737 

25.953 

0.386 

2.086 

68.800 

63.108 

26.103 

0.383 

2.084 

69.200 

63.479 

26.252 

0.380 

2.083 

69.600 

63.851 

26.400 

0.377 

2.081 

70.000 

64.223 

26.546 

0.374 

2.080 

70.400 

64.596 

26.692 

0.371 

2.078 

70.800 

64.969 

26.836 

0.368 

2.077 

71.200 

65.342 

26.979 

0.365 

2.075 

71.600 

65.716 

27.121 

0.362 

2.073 

72.000 

66.091 

27.262 

0.358 

2.072 

72.400 

66.465 

27.402 

0.355 

2.070 

72.800 

66.841 

27.541 

0.352 

2.069 

73.200 

67.216 

27.678 

0.349 

2.067 

73.600 

67.592 

27.815 

0.347 

2.066 

74.000 

67.969 

27.950 

0.344 

2.064 

74.400 

68.346 

28.084 

0.341 

2.062 

74.800 

68.723 

28.217 

0.338 

2.061 

75.200 

69.100 

28.349 

0.335 

2.059 

75.600 

69.478 

28.480 

0.332 

2.058 

76.000 

69.857 

28.610 

0.329 

2.056 

76.400 

70.236 

28.738 

0.326 

2.055 

76.800 

70.615 

28.866 

0.323 

2.053 

77.200 

70.994 

28.992 

0.320 

2.051 

77.600 

71.374 

29.117 

0.317 

2.050 

78.000 

71.754 

29.241 

0.314 

2.048 

78.400 

72.135 

29.365 

0.311 

2.047 

78.800 

72.516 

29.487 

0.308 

2.045 

79.200 

72.897 

29.607 

0.306 

2.044 
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79.600 

73.279 

29.727 

0.303 

2.042 

80.000 

73.661 

29.846 

0.300 

2.040 

80.400 

74.043 

29.964 

0.297 

2.039 

80.800 

74.426 

30.080 

0.294 

2.037 

81.200 

74.809 

30.196 

0.292 

2.036 

81.600 

75.192 

30.310 

0.289 

2.034 

82.000 

75.576 

30.424 

0.286 

2.033 

82.400 

75.959 

30.536 

0.283 

2.031 

82.800 

76.344 

30.647 

0.281 

2.029 

83.200 

76.728 

30.757 

0.278 

2.028 

83.600 

77.113 

30.867 

0.275 

2.026 

84.000 

77.498 

30.975 

0.272 

2.025 

84.400 

77.883 

31.082 

0.270 

2.023 

84.800 

78.269 

31.188 

0.267 

2.022 

85.200 

78.655 

31.293 

0.264 

2.020 

85.600 

79.041 

31.397 

0.262 

2.018 

86.000 

79.428 

31.500 

0.259 

2.017 

86.400 

79.815 

31.602 

0.257 

2.015 

86.800 

80.202 

31.703 

0.254 

2.014 

87.200 

80.589 

31.803 

0.251 

2.012 

87.600 

80.976 

31.902 

0.249 

2.011 

88.000 

81.364 

32.000 

0.246 

2.009 

88.400 

81.752 

32.097 

0.244 

2.008 

88.800 

82.141 

32.193 

0.241 

2.006 

89.200 

82.529 

32.289 

0.239 

2.004 

89.600 

82.918 

32.383 

0.236 

2.003 

90.000 

83.307 

32.476 

0.234 

2.001 

90.400 

83.696 

32.568 

0.232 

2.000 

90.800 

84.085 

32.659 

0.229 

1.998 
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91.200 

84.475 

32.750 

0.227 

1.997 

91.600 

84.865 

32.839 

0.224 

1.995 

92.000 

85.255 

32.928 

0.222 

1.993 

92.400 

85.645 

33.015 

0.220 

1.992 

92.800 

86.036 

33.102 

0.217 

1.990 

93.200 

86.427 

33.188 

0.215 

1.989 

93.600 

86.817 

33.273 

0.213 

1.987 

94.000 

87.209 

33.357 

0.210 

1.986 

94.400 

87.600 

33.440 

0.208 

1.984 

94.800 

87.991 

33.522 

0.206 

1.982 

95.200 

88.383 

33.603 

0.204 

1.981 

95.600 

88.775 

33.684 

0.201 

1.979 

96.000 

89.167 

33.763 

0.199 

1.978 

96.400 

89.559 

33.842 

0.197 

1.976 

96.800 

89.951 

33.920 

0.195 

1.975 

97.200 

90.344 

33.997 

0.193 

1.973 

97.600 

90.736 

34.073 

0.191 

1.971 

98.000 

91.129 

34.149 

0.189 

1.970 

98.400 

91.522 

34.223 

0.186 

1.968 

98.800 

91.915 

34.297 

0.184 

1.967 

99.200 

92.309 

34.370 

0.182 

1.965 

99.600 

92.702 

34.442 

0.180 

1.964 

100.000 

93.096 

34.513 

0.178 

1.962 

100.400 

93.489 

34.584 

0.176 

1.960 

100.800 

93.883 

34.654 

0.174 

1.959 

101.200 

94.277 

34.723 

0.172 

1.957 

101.600 

94.671 

34.791 

0.170 

1.956 

102.000 

95.066 

34.858 

0.168 

1.954 

102.400 

95.460 

34.925 

0.166 

1.953 
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102.800 

95.855 

34.991 

0.165 

1.951 

103.200 

96.249 

35.056 

0.163 

1.949 

103.600 

96.644 

35.120 

0.161 

1.948 

104.000 

97.039 

35.184 

0.159 

1.946 

104.400 

97.434 

35.247 

0.157 

1.945 

104.800 

97.829 

35.309 

0.155 

1.943 

105.200 

98.224 

35.371 

0.154 

1.942 

105.600 

98.620 

35.432 

0.152 

1.940 

106.000 

99.015 

35.492 

0.150 

1.938 

106.400 

99.411 

35.551 

0.148 

1.937 

106.800 

99.806 

35.610 

0.146 

1.935 

107.200 

100.202 

35.668 

0.145 

1.934 

107.600 

100.598 

35.725 

0.143 

1.932 

108.000 

100.994 

35.782 

0.141 

1.931 

108.400 

101.390 

35.838 

0.140 

1.929 

108.800 

101.786 

35.893 

0.138 

1.927 

109.200 

102.182 

35.948 

0.136 

1.926 

109.600 

102.579 

36.002 

0.135 

1.924 

110.000 

102.975 

36.056 

0.133 

1.923 

110.400 

103.372 

36.108 

0.132 

1.921 

110.800 

103.768 

36.161 

0.130 

1.920 

111.200 

104.165 

36.212 

0.128 

1.918 

111.600 

104.562 

36.263 

0.127 

1.916 

112.000 

104.959 

36.313 

0.125 

1.915 

112.400 

105.355 

36.363 

0.124 

1.913 

112.800 

105.752 

36.412 

0.122 

1.912 

113.200 

106.149 

36.461 

0.121 

1.910 

113.600 

106.547 

36.509 

0.119 

1.909 

114.000 

106.944 

36.556 

0.118 

1.907 
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114.400 

107.341 

36.603 

0.116 

1.905 

114.800 

107.738 

36.649 

0.115 

1.904 

115.200 

108.136 

36.695 

0.114 

1.902 

115.600 

108.533 

36.740 

0.112 

1.901 

116.000 

108.931 

36.784 

0.111 

1.899 

116.400 

109.328 

36.828 

0.109 

1.898 

116.800 

109.726 

36.872 

0.108 

1.896 

117.200 

110.124 

36.915 

0.107 

1.894 

117.600 

110.521 

36.957 

0.105 

1.893 

118.000 

110.919 

36.999 

0.104 

1.891 

118.400 

111.317 

37.040 

0.103 

1.890 

118.800 

111.715 

37.081 

0.102 

1.888 

119.200 

112.113 

37.121 

0.100 

1.887 

119.600 

112.511 

37.161 

0.099 

1.885 

120.000 

112.909 

37.200 

0.098 

1.883 

120.400 

113.307 

37.239 

0.097 

1.882 

120.800 

113.705 

37.277 

0.095 

1.880 

121.200 

114.103 

37.315 

0.094 

1.879 

121.600 

114.502 

37.353 

0.093 

1.877 

122.000 

114.900 

37.389 

0.092 

1.876 

122.400 

115.298 

37.426 

0.091 

1.874 

122.800 

115.697 

37.462 

0.089 

1.872 

123.200 

116.095 

37.497 

0.088 

1.871 

123.600 

116.494 

37.532 

0.087 

1.869 

124.000 

116.892 

37.567 

0.086 

1.868 

124.400 

117.291 

37.601 

0.085 

1.866 

124.800 

117.689 

37.635 

0.084 

1.865 

125.200 

118.088 

37.668 

0.083 

1.863 

125.600 

118.486 

37.701 

0.082 

1.861 
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APPENDIX  M:  SENSING  SIMULATION  CODE  (MAIN2.CC) 


//  File:  main2.cc 

//  Name:  Edward  Mays 

//  Sensing  Simulation 

//  Unix 

//GCC 

// Date:  26  August  1997 

// 

//  Description 

//  THIS  PROGM  SIMULATES  THE  MOVEMENT  OF  A  SQARE  OBJECT  ALONG  A 
//PATH.  THE  OBJECT'S  PATH  DIRECTION  (THETA)  IS  CHANGING,  AS  IS  THE 
//OBJECTS  ORIENTATION  (PSI).  LINE  TRACKING  IS  USED  AND  THE  X-AXIS  IS 
THE  //REFERENCE  LINE.  THE  REFERENCE  LINE  IS  INCREMENTED  BY  40 
UNITS  IN  THE 

// 

//  Header  file  info 

// 

#include  <iostream.h> 
#include  <math.h> 
#include  <fstream.h> 
#include  <stdio.h> 


#define  PI      3.141 59265358979323846 
#define  RAD     57.2957795 1 308232087684 
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double  deltaTime  =  0.01;//  0.01 

double  Vel  =  40.0; 

double  omega  =  -0.1570796327; 

FILE  *f0,  *f  1 ,  *f2,  *f3,  *f4,  *f5,*f6  ;        //PTR  TO  FILE  FOR  OUTPUT  DATA 


//structure  to  hold  configuration  including  x,  y,  theta,  and  kappa 

typedef  struct  { 
double  x; 
double  y;    } 
POINT; 

typedef  struct{ 
POINT  Point; 
double  Theta; 
double  Kappa; 
double  Psi; 

} 
CONFIGURATION; 


// 

//Function:  GetSmooth 
//Return  Value :n/a 
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//Parameters:  function  parm  list 

//Purpose:  gets  users  input  for  sO/smoothness 
// 

double  GetSmooth(double  &s0) 

{ 


cout  «  "enter  your  value  for  smoothness  (negatives  not  allowed)"  «endl; 
cin  »  sO; 
return  (sO  >=  0.0); 

}//  GetSmooth 

// 

//Function:  InitConfig 

//Return  Value:n/a 

//Parameters:  function  parm  list 

//Purpose:  SETS  INITIAL  CONFIGURATION 

// 

void  InitConfig(CONFIGURATION&  q.init,  CONFIGURATION& 

q_xaxis,CONFIGURATION&  qbody,CONFIGURATION&  qfrontR, 

CONFIGURATION&  qfrontL,  CONFIGURATION&  qrearR,  CONFIGURATION& 
qrearL,  CONFIGURATION&  qsnapshot, 

double  &s0,  double  &deltaS) 

{ 

cout«"Setting  the  initial  configuration" 
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«"x=0,  y  =  0,  theta  =  0,  and  kappa  =  0  "«endl; 

q_init.Point.x  =  0.0; 

q_init.Point.y  =  0.0; 

q_init.Theta    =  0.0; 

q_init.Kappa    =  0.0; 

q_init.Psi    =  2.356219449;  /*  3*PI/4.0  */ 

cout«"Setting  the  reference  line  configuration" 
«"x  =  0,  y  =  40,  theta  =  0  ,  and  kappa  =  0  "«endl; 

q_xaxis.Point.x  =  0.0; 
q_xaxis.Point.y  =  40.0; 
q_xaxis. Theta     =  0.0; 
q_xaxis. Kappa    =  0.0; 


//individual  wheels 
qfrontR.Point.x  =  40;    /*  wheel  1  */ 
qfrontR. Point. y  =  -40; 

qfrontL.Point.x  =  40;    /*  wheel2  */ 
qfrontL.Point.y  =  40; 

qrearR.Point.x  =  -40;    /*  wheel3  */ 
qrearR.  Point,  y  =  -40; 

268 


qrearL.Point.x  =  -40;    /*  wheel  4  */ 
qrearL.Point.y  =  40; 

qsnapshot.Point.x  =  0.0; 
qsnapshot.Point.y  =  0.0; 

cout«"Enter  size  constant  for  smoothness  <return>"«endl; 
GetSmooth(sO); 

cout« "Entering  Step  size  constant  deltaS(deltaS=Vel*deltaT)."«endl; 
deltaS  =  Vel*deltaTime;//.05  orig 


}//  InitConfig 


// 

//Function:  CreateConst 

//Return  Value:n/a 

//Parameters:  function  parm  list 

//Purpose:  create  constants  for 

//           steering  function  dk/ds 
// 

void  CreateConst(double  &a,  double  &b,  double  &c,  double  &s0) 
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{ 

double  k; 

k  =  1 .0/sO;  //all  consts  by  def,  including  curvature 
a  =  3.0*k; 
b  =  3.0*k*k; 
c  =  k*k*k; 
}//  CreateConst 


// 

//Function:  GetSteerL 

//Return  Value:n/a 

//Parameters:  function  parm  list 

//Purpose:      lambda=dk/ds  (LINEAR  STEERING  FUNCTION) 

// 

double  GetSteerL(double  &a,  double  &b,  double  &c,  CONFIGURATION&  q, 

CONFIGURATION&  q_xaxis) 
{ 
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double  delta_r; 

delta_r  =  -(q.Point.x  -  q_xaxis.Point.x)*sin(q_xaxis.Theta)  + 
(q.Point.y  -  q_xaxis.Point.y)*cos(q_xaxis.Theta); 

return  (-(a*q. Kappa  +  b*(q.Theta  -  q_xaxis.Theta)  +  c*delta_r)); 
}  //GetSteerL 


// 

//Function:  GetDeltakappa 

//Return  Value  :n/a 

//Parameters:  function  parm  list 

//Purpose:    DETERMINES  THE  KAPPA  DIFFERENCE  PER  INCREMENT  OF  S 

// 

double  GetDeltakappa(double  &Dk_Ds,  double  &deltaS,  double  &deltakappa) 

{ 

deltakappa  =  Dk_Ds*deltaS; 

return(deltakappa); 

}//GetDeltakappa 
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// 

//Function:  returnkappa 

//Return  Value  :n/a 

//Parameters:  function  parm  list 

//Purpose:  CALCULATES  NEW  VALUE  FOR  KAPPA  USING  deltaK 

// 

CONFIGURATION  returnkappa(double  &deltakappa,  CONFIGURATION  &q) 

{ 

q. Kappa  =  q. Kappa  +  deltakappa; 

return  q; 

} //returnkappa 


// 

//Function:  GetS 

//Return  Value :n/a 

//Parameters:  function  parm  list 

//Purpose:  INCREMENTS  S  THROUGH  EACH  ITERATION  OF  THE  WHILE  LOOP 
// 

double  GetS(double  &s,  double  &deltaS) 
{ 
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s  =  s  +  deltaS ; 
return  s; 

}//GetS 

// 

//Function:  GetDeltaTheta 

//Return  Value :n/a 

//Parameters:  function  parm  list 

//Purpose:  COMPUTES  CHANGE  IN  THETA  PER  INCREMENT  OF  S 

// 

double  GetDeltaTheta(CONFIGURATION  &q,  double  &deltaS,  double  &deltaT) 

{ 

deltaT  =  q.  Kappa*  deltaS; 

return(deltaT); 

} //GetDeltaTheta 

//  — 

//Function:  Circ 

//Return  Value:n/a 

//Parameters:  function  parm  list 

//Purpose:  Circ  function  from  notes  6.29 
// 
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void  Circ(double  Length,  double  alpha,  CONFIGURATION  &q) 

{ 

double  alpha2,  alpha4; 

alpha2=alpha*  alpha; 
alpha4=alpha2*alpha2; 


//configuration  ql 

q.Point.x  =  (1.0  -  alpha2/6.0  +  alpha4/120.0)*Length; 
q.Point.y  =  (0.5  -  alpha2/24.0  +  alpha4/720.0)*Length*alpha; 
q.Theta  =  alpha; 


}//Circ 


// 

//Function:  Compose 

//Return  Value:n/a 

//Parameters:  function  parm  list 

//Purpose:  updates  the  configuration  and  computes  new  config  (notes  6.2) 

// 

CONFIGURATION         Compose(CONFIGURATION&         q  1  ,CONFIGURATION& 
q2,CONFIGURATION&  q3,  double&  s,double&  deltaTime) 
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{    double  x,y, 

sinTheta  =  sin(ql.Theta), 

cosTheta  =  cos(q  1  .Theta); 

x  =  ql.Point.x  +  q2. Point. x*cosTheta  -  q2.Point.y*sinTheta; 

y  =  ql.Point.y  +  q2.Point.x*sinTheta  +  q2.Point.y*cosTheta; 

q3.Point.x  =  x; 

q3. Point. y  =  y; 

q3  .Theta  =  ql. Theta  +  q2. Theta; 

q3.Psi  =  ql .Psi  +  (omega  *  deltaTime);    /*  how  to  handle  move  left/right?  */ 

fprintf(f6,"%10.3f  %10.3f  %10.3f  %10.3f  %10.3f\n", 

s,q3.Point.x,  q3.Point.y,q3.Theta,  q3.Psi); 
return  q3; 

}//  end  Compose 


CONFIGURATION    Compose2(CONFIGURATION&    q  1  ,CONFIGURATION&    q2, 
CONFIGURATION&  q3)  /*position  */ 

{    double  x,y, 

sinTheta  =  sin(ql.Psi), 

cosTheta  =  cos(q  1  .Psi); 
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x  =  ql.Point.x  +  q2.Point.x*cosTheta  -  q2.Point.y*sinTheta; 
y  =  ql.Point.y  +  q2.Point.x*sinTheta  +  q2.Point.y*cosTheta; 

q3.Point.x  =  x; 
q3.Point.y  =  y; 
return  q3; 
}//  end  Compose2 


// 

//Function:  Openfile 
//Return  Value:n/a 
//Parameters:  function  parm  list 

//Purpose:  To  compute  transposition 
// 

void  Openfile() 

{ 

fO  =  fopen("drk.dat",  "w"); 

fl  =fopen("wheell.dat","w"); 

f2  =  fopen("wheel2.dat","wM); 

f3  =  fopen("wheel3.dat","w"); 
f4  =  fopen("wheel4.dat","wM); 

f5  =  fopen("composite.dat", "w"); 
f6  =  fopenC'psi.dat'V'w"); 

}//  Openfile 
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// 

//Function:  Print 
//Return  Value  :n/a 
//Parameters:  function  parm  list 
//Purpose:  To  compute  transposition 
// 

void  printFile(FILE  *f, CONFIGURATION  &q) 

{ 

fprintf(f,"%10.3f%10.3f\n", 
q.Point.x,  q.Point.y); 


}//  printFile 


// 

//Function:  blankLine 
//Return  Value :n/a 
//Parameters:  function  parm  list 
//Purpose:  To  compute  transposition 
// 

void  blankLine(FILE  *f) 
{ 
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fprintf(f,"\n"); 


}//  blankLine 


// 

//Function:  update  Wheels 

//Return  Value:n/a 

//Parameters:  function  parm  list 

/Purpose:  create  constants  for 

//           steering  function  dk/ds 
// 

void  updateWheels(CONFIGURATION&  qbody,  CONFIGURATION&  qfrontR, 
CONFIGURATION&  qfrontL,  CONFIGURATION  qrearR,  CONFIGURATION  qrearL, 
CONFIGURATION&  qwheell,  CONFIGURATION&  qwheel2,  CONFIGURATION& 
qwheeB,  CONFIGURATION&  qwheel4,  CONFIGURATION&  q3,int&  s2) 

{      printFile(fO,qbody); 

qwheell  =  Compose2(qbody,qfrontR,q3); 

printFile(f  1  ,qwheel  1 ); 

blankLine(fl); 

qwheel2  =  Compose2(qbody,qfrontL,q3); 

printFile(f2,qwheel2); 
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blankLine(f2); 

qwheeB  =  Compose2(qbody,  qrearR,  q3); 

printFile(f3,qwheel3); 

blankLine(f3); 

qwheel4  =  Compose2(qbody,  qrearL,q3); 

printFile(f4,qwheel4); 

blankLine(f4); 

if ((s2=0.0)ll(s2%  1 00==0))  { 


printFile(f5,qwheel  1 ) 
printFile(f5,qwheel2) 
printFile(f 5  ,q  wheel4) 
printFile(f5, qwheeB) 
printFile(f5,qwheel  1 ) 
blankLine(f5); 

} 

}//  updateWheels 


int  main() 
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{   CONFIGURATION  q,  q_xaxis,  New_q,  qbody,  qfrontR,  qfrontL,  qrearR,  qrearL, 
qwheell,  qwheel2,  qwheeB,  qwheel4,  qsnapshot,q3; 


int  ix,s2,counter; 

double  a,  b,  c; 

double  s,sO; 

double  Dk_Ds; 

double  deltaS ; 

double  deltaT; 

double  deltaK; 

const  double  Sdig  =  0.001 ; 

double  smax=400.0; 


//constants  equation  6.3 

//s0  is  smoothness,  s  is  the  incremental  step 


//const  used  for  prec/toler 


InitConfig(q,  q_xaxis,    qbody,  qfrontR,  qfrontL,  qrearR,  qrearL,qsnapshot,sO,  deltaS); 
//configuaration  set  up 


Openfile(); 
printFile(fO,q); 
printFile(f  1  ,qfrontR); 
printFile(f2,qfrontL); 
printFile(f3,qrearR); 
printFile(f4,qrearL); 


//write  initial  config  to  file 


//printFile(f5,qfrontR); 
//printFile(f5,qfrontL); 
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//printFile(f5,qrearL); 
//printFile(f5,qrearR); 

printFile(f5,qfrontR); 
blankLine(f5); 

CreateConst(a,  b,  c,  sO);  //calcs  consts 

s  =  0.0; 

s2=s; 

counter=0; 

for(ix=0;  ix<10;ix++){ 

do 

{// 

Dk_Ds  =  GetSteerL(a,  b,  c,  q,  q_xaxis);  //calculates  lambda  =dk/ds 

deltaK  =  GetDeltakappa(Dk_Ds,  deltaS,  deltaK);    //lambda*deltaS 
returnkappa(deltaK,  q);  //Kappa  <=  kappa  +  deltaK 

deltaT  =  GetDeltaTheta(q,  deltaS,  deltaT);        //Theta  <=  Theta  +  deltaT 
Circ(deltaS,  deltaT,  New_q);  //cir 

qbody  =  Compose(q,  New_q,  q,  s,  deltaTime);  //compose 

updateWheels(qbody,qfrontR,  qfrontL,  qrearR, 

qrearL,qwheel  1 ,  qwheel2,  qwheeB, 

qwheel4,q3,s2); 
GetS(s,  deltaS); 
s2=s; 
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}  while  (s<smax); 

//  }  while  ((fabs(q.Point.y)  >  Sdig)ll(fabs(q.Theta)  >  Sdig) 

//         (fabs(q.Kappa)  >  Sdig)); 

s=0.0; 

s2=s; 

q_xaxis. Point. y  =  q_xaxis.Point.y  +  40.0; 

if(ix%2==0){ 

q_xaxis.Theta     =  PI; 

q.Theta     =  PI; 

omega  =  fabs(omega); 

}else{ 

q_xaxis.Theta     =  0.0; 

q.Theta     =  0.0; 

omega  =  -omega; 

} 

} 

fclose(fO); 

fclose(fl); 

fclose(f2); 

fclose(O); 

fclose(f4); 

fclose(f5); 

fclose(f6); 

return  0; 

}//end  main2.cc 
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APPENDIX  N:  INPUT  VS.  OUPUT  VELOCITY 


-1024-87.657 
-1023  -87.429 
-1020-86.975 
-1015  -86.747 
-1010-86.406 
-1005  -85.838 
-1000-85.383 
-900  -76.856 
-800  -67.998 
-700  -59.082 
-600-51.048 
-500  -42.748 
-400-34.107 
-375-31.834 
-350  -29.673 
-300  -25.580 
-250-21.260 
-225-19.100 
-200-17.053 
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-175-14.780 
-150-12.506 
-125  -10.459 
-100-8.413 
-90  -7.503 
-80  -6.707 
-70  -5.798 
-60  -5.002 
-50  -4.092 
-40  -3.297 
-30  -2.387 
-20-1.591 
-10 -.682 
-5 -.341 
-1  -.113 
0.0  0.0 
1023  87.429 
1020  86.975 
1015  86.747 
1010  86.406 
1005  85.838 
1000  85.383 
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900  76.856 
800  67.998 
700  59.082 
600  51.048 
500  42.748 
400  34.107 
375  31.834 
350  29.673 
300  25.580 
250  21.260 
225  19.100 
200  17.053 
175  14.780 
150  12.506 
125  10.459 
100  8.413 
90  7.503 
80  6.707 
70  5.798 
60  5.002 
50  4.092 
40  3.297 
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30  2.387 
20  1.591 
10  .682 
5.341 
1  .113 
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APPENDIX  O:  INPUT  VS  OUTPUT  STEERING  RATES 

A.      DESIRED  INPUT  RATE  VS  ACTUAL  (BOTH  ESTIMATED  AND 
SOFTWARE  DEPENDENT) 


Desired  rate  of 
turn 

Time 
Stop  watch 

(sec) 

Estimated  Rate 

(rad/s) 

Software 
Measured  Rate 
(rad/s,  average) 

1 

6.0 

1.00000 

0.98174 

2 

3.5 

1.79485 

1.95667 

3 

2.19 

2.86849 

2.93160 

5 

1.69 

2.71716 

3.90653 

5.5 

No  data 

No  data 

4.88828 

10 

No  data 

No  data 

5.23598 

20 

No  data 

No  data 

5.23598 

30 

No  data 

No  data 

5.23598 

Figure  A.l  Inputs  and  results  from  massaged  data  (error).     No  data  entries  exist  because  the 
revolutions  were  too  fast  for  hand  timing. 


B.         DESIRED  INPUT  RATE  VS  OUPUT  FOR  EACH  WHEEL  (SOFTWARE 
DEPENDENT 

Below  M5,  M6,  M7,  and  M8  reperesent  the  steering  motors  for  wheel  1,  wheel  2, 
wheeB,  and  wheel4  respectively. 


Desired  Rate 
of  turn 

M5  Rate 

M6  Rate 

M7  Rate 

M8  Rate 

1 

1.002 

.9975 

.9965 

.999 

2 

2.0025 

1.997 

1.990 

1.997 

3 

3.005 

2.995 

2.9975 

3.005 

4 

4.004 

3.996 

3.9925 

3.996 

5 

5.008 

5.002 

4.9985 

5.002 
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Desired  Rate 
of  turn 

M5  Rate 

M6  Rate 

M7  Rate 

M8  Rate 

5.1 

5.1035 

5.0935 

5.093 

5.0955 

5.2 

5.2065 

5.198 

5.1955 

5.198 

5.3 

5.238 

5.235 

5.234 

5.235 

5.4 

5.238 

5.235 

5.234 

5.235 

-1 

-1.001 

-1.002 

-1.002 

-1.002 

-2 

-2.001 

-2.003 

-2.003 

-2.004 

-3 

-3.0055 

-3.006 

-3.0055 

-3.006 

-4 

-4.007 

-4.006 

-4.0015 

-4.008 

-5 

-5.010 

-5.010 

-5.009 

-5.010 

-5.1 

-5.107 

-5.110 

-5.1035 

-5.112 

-5.2 

-5.2105 

-5.2125 

-5.2075 

-5.214 

-5.3 

-5.238 

-5.2415 

-5.237 

-5.2535 

-5.4 

-5.238 

-5.2415 

-5.237 

-5.2535 

Figure  A.2:  Desired  (commanded)    rate  of  turn  vs.  actual  "free  floating"  motor  rate. 


C.         DESIRED  INPUT  (DIGIT  MANIPULATION)  VS  OUTPUT  RATE 


Input 
(digits) 

M5  Rate 

(rad/s) 

M6  Rate 

(rad/s) 

M7  Rate 
(rad/s) 

M8  Rate 

(rad/s) 

10 

.056 

.045 

.044 

.045 

20 

.102 

.101 

.100 

.100 

30 

.152 

.143 

.143 

.143 

40 

.205 

.203 

.203 

.203 

50 

.261 

.250 

.248 

.249 

60 

.306 

.306 

.305 

.305 

70 

.363 

.350 

.349 

.350 

80 

.409 

.408 

.407 

.407 

90 

.466 

.454 

.452 

.455 

100 

.511 

.510 

.510 

.510 

200 

1.024 

1.022 

1.022 

1.022 
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Input 
(digits) 

M5  Rate 
(rad/s) 

M6  Rate 

(rad/s) 

M7  Rate 

(rad/s) 

M8  Rate 

(rad/s) 

300 

1.539 

1.533 

1.533 

1.533 

400 

2.049 

2.046 

2.045 

2.045 

500 

2.561 

2.556 

2.556 

2.556 

600 

3.074 

3.068 

3.067 

3.068 

700 

3.584 

3.579 

3.579 

3.579 

800 

4.097 

4.092 

4.091 

4.092 

900 

4.610 

4.602 

4.602 

4.602 

1000 

5.124 

5.116 

5.114 

5.116 

1010 

5.174 

5.172 

5.170 

5.172 

1020 

5.226 

5.218 

5.216 

5.218 

1021 

5.233 

5.226 

5.222 

5.225 

1022 

5.235 

5.233 

5.231 

5.232 

1023 

5.237 

5.235 

5.234 

5.235 

Figure  A.3:  Desired  (commanded)    rate  of  turn  vs.  actual  "free  floating"  motor  rate  for  each  wheel 
using  input  digits . 


D. 


WHEEL  4  ROTATION  DATA 


Clockwise  Rotation 

Counterclockwise 
Rotation 

1 

000.867 

360.390 

2 

000.878 

360.390 

3 

000.976 

360.363 

4 

000.933 

360.414 

5 

000.984 

360.371 

6 

000.992 

360.394 

7 

000.992 

360.453 

8 

000.902 

360.394 

9 

000.996 

360.445 

10 

000.996 

360.476 

11 

001.003 

360.402 
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Clockwise  Rotation 

Counterclockwise 
Rotation 

12 

000.917 

360.417 

13 

000.996 

360.433 

14 

001.003 

360.468 

15 

000.972 

360.468 

16 

000.863 

360.472 

17 

000.968 

360.398 

18 

000.941 

360.425 

19 

000.957 

360.480 

20 

000.937 

360.480 

Figure  A.4:  Wheel  4  data  based  on  position  of  rest  after  direction  of  turn. 
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